diff --git a/src/无人机端代码/Modules/common/CMakeLists.txt b/src/无人机端代码/Modules/common/CMakeLists.txt deleted file mode 100644 index 66dd650a..00000000 --- a/src/无人机端代码/Modules/common/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/include/geometry_utils.h b/src/无人机端代码/Modules/common/include/geometry_utils.h deleted file mode 100644 index a9b77726..00000000 --- a/src/无人机端代码/Modules/common/include/geometry_utils.h +++ /dev/null @@ -1,248 +0,0 @@ -#ifndef __GEOMETRY_UTILS_H -#define __GEOMETRY_UTILS_H - -#include - -/* clang-format off */ -namespace geometry_utils { - -template -Scalar_t toRad(const Scalar_t& x) { - return x / 180.0 * M_PI; -} - -template -Scalar_t toDeg(const Scalar_t& x) { - return x * 180.0 / M_PI; -} - -template -Eigen::Matrix rotx(Scalar_t t) { - Eigen::Matrix 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 -Eigen::Matrix roty(Scalar_t t) { - Eigen::Matrix 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 -Eigen::Matrix rotz(Scalar_t t) { - Eigen::Matrix 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 -Eigen::Matrix ypr_to_R(const Eigen::DenseBase& 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 Rz = Eigen::Matrix::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 Ry = Eigen::Matrix::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 Rx = Eigen::Matrix::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 R = Rz * Ry * Rx; - return R; -} - -template -Eigen::Matrix R_to_ypr(const Eigen::DenseBase& 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 n = R.col(0); - Eigen::Matrix o = R.col(1); - Eigen::Matrix a = R.col(2); - - Eigen::Matrix 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 -Eigen::Quaternion ypr_to_quaternion(const Eigen::DenseBase& 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 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 -Eigen::Matrix quaternion_to_ypr(const Eigen::Quaternion& q_) { - Eigen::Quaternion q = q_.normalized(); - - Eigen::Matrix 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 -Scalar_t get_yaw_from_quaternion(const Eigen::Quaternion& q) { - return quaternion_to_ypr(q)(0); -} - -template -Eigen::Quaternion yaw_to_quaternion(Scalar_t yaw) { - return Eigen::Quaternion(rotz(yaw)); -} - -template -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 -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 -Scalar_t yaw_add(Scalar_t a, Scalar_t b) { - return angle_add(a, b); -} - -template -Eigen::Matrix get_skew_symmetric(const Eigen::DenseBase& 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 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 -Eigen::Matrix from_skew_symmetric(const Eigen::DenseBase& 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 v; - v(0) = M(2, 1); - v(1) = -M(2, 0); - v(2) = M(1, 0); - - assert(v.isApprox(Eigen::Matrix(-M(1, 2), M(0, 2), -M(0, 1)))); - - return v; -} - - -} // end of namespace geometry_utils -/* clang-format on */ - -#endif diff --git a/src/无人机端代码/Modules/common/include/math_utils.h b/src/无人机端代码/Modules/common/include/math_utils.h deleted file mode 100644 index d46a57ed..00000000 --- a/src/无人机端代码/Modules/common/include/math_utils.h +++ /dev/null @@ -1,127 +0,0 @@ -#ifndef MATH_UTILS_H -#define MATH_UTILS_H - -#include -#include - -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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/include/printf_utils.h b/src/无人机端代码/Modules/common/include/printf_utils.h deleted file mode 100644 index 941fbf68..00000000 --- a/src/无人机端代码/Modules/common/include/printf_utils.h +++ /dev/null @@ -1,132 +0,0 @@ -#ifndef PRINTF_UTILS_H -#define PRINTF_UTILS_H - -#include -#include -#include -#include -#include -using namespace std; - -#define NUM_POINT 2 // 打印小数点 - -#define RED "\033[0;1;31m" -#define GREEN "\033[0;1;32m" -#define YELLOW "\033[0;1;33m" -#define BLUE "\033[0;1;34m" -#define PURPLE "\033[0;1;35m" -#define DEEPGREEN "\033[0;1;36m" -#define WHITE "\033[0;1;37m" - -#define RED_IN_WHITE "\033[0;47;31m" -#define GREEN_IN_WHITE "\033[0;47;32m" -#define YELLOW_IN_WHITE "\033[0;47;33m" - -#define TAIL "\033[0m" - -class Print -{ -public: - Print(float interval = 0, std::string color = TAIL) - : interval(interval), past_ts(std::chrono::system_clock::now()), color(color) - { - //固定的浮点显示 - std::cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为n位 - std::cout << std::setprecision(2); - //左对齐 - std::cout.setf(ios::left); - // 强制显示小数点 - std::cout.setf(ios::showpoint); - // 强制显示符号 - std::cout.setf(ios::showpos); - }; - - template - void operator()(T &&msg) - { - std::chrono::system_clock::time_point now_ts = std::chrono::system_clock::now(); - auto dt = std::chrono::duration_cast(now_ts - past_ts).count(); - - if (this != s_object_name) - { - std::cout << std::endl; - s_object_name = this; - } - - if (interval >= 0) - { - // std::cout << this->interval << std::endl; - if (dt < this->interval * 1000) - return; - std::cout << color << msg << TAIL << std::endl; - } - else - { - if (dt < 0.1 * 1000) - return; - char now_char; - switch (times) - { - case 0: - now_char = '\\'; - break; - case 1: - now_char = '|'; - break; - case 2: - now_char = '/'; - break; - case 3: - now_char = '-'; - break; - } - times = ++times % 4; - std::cout << color << "\r " << now_char << " " << msg << TAIL << std::flush; - } - this->past_ts = now_ts; - }; - - float interval; - -private: - std::chrono::system_clock::time_point past_ts; - std::string color; - static void *s_object_name; - unsigned int times = 0; -}; - -void *Print::s_object_name = nullptr; - -#define PRINTF_UTILS_CONCAT_(x, y) x##y -#define PRINTF_UTILS_CONCAT(x, y) PRINTF_UTILS_CONCAT_(x, y) - -#define PRINTF_UTILS_PCOUT_(interval, color, msg, sign) \ - static Print PRINTF_UTILS_CONCAT(print, sign)(interval, color); \ - PRINTF_UTILS_CONCAT(print, sign) \ - (msg) - -#define PCOUT(interval, color, msg) PRINTF_UTILS_PCOUT_(interval, color, msg, __LINE__) - -// Example: -// cout << GREEN << "Test for Green text." << TAIL < Hightlight color.\033[0m"); -// 其中41的位置代表字的背景色, 30的位置是代表字的颜色，0 是字的一些特殊属性，0代表默认关闭，一些其他属性如闪烁、下划线等。 - -// ROS_INFO_STREAM_ONCE ("\033[1;32m---->Setting to OFFBOARD Mode....\033[0m");//绿色只打印一次 - -#endif diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/CMakeLists.txt b/src/无人机端代码/Modules/common/prometheus_msgs/CMakeLists.txt deleted file mode 100644 index 3c852583..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/CMakeLists.txt +++ /dev/null @@ -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 -) diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/action/CheckForObjects.action b/src/无人机端代码/Modules/common/prometheus_msgs/action/CheckForObjects.action deleted file mode 100644 index ab6b2b6a..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/action/CheckForObjects.action +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/ArucoInfo.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/ArucoInfo.msg deleted file mode 100644 index afd65e01..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/ArucoInfo.msg +++ /dev/null @@ -1,17 +0,0 @@ -std_msgs/Header header - -## aruco编码 -int32 aruco_num - -## 是否检测到目标 -bool detected - -## 目标位置-相机坐标系-(x,y,z) -## 从相机往前看，物体在相机右方x为正，下方y为正，前方z为正 -float32[3] position - -## 目标姿态-四元数-(qx,qy,qz,qw) -float32[4] orientation - -## 视线角度-相机系下-(右方x角度为正,下方y角度为正) -float32[2] sight_angle diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/BoundingBox.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/BoundingBox.msg deleted file mode 100644 index 9b738721..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/BoundingBox.msg +++ /dev/null @@ -1,8 +0,0 @@ -# 目标框相关信息 -string Class # 类别 -float64 probability # 置信度 -int64 xmin # 右上角 -int64 ymin -int64 xmax # 坐下角 -int64 ymax - diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/BoundingBoxes.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/BoundingBoxes.msg deleted file mode 100644 index 3ec3fe82..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/BoundingBoxes.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -Header image_header -BoundingBox[] bounding_boxes diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/DetectionInfo.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/DetectionInfo.msg deleted file mode 100644 index 452abd6a..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/DetectionInfo.msg +++ /dev/null @@ -1,33 +0,0 @@ -# 目标信息 - -std_msgs/Header header - -## 目标类别名称 -string object_name - -## 是否检测到目标 -bool detected - -## 0表示相机坐标系, 1表示机体坐标系, 2表示惯性坐标系 -int32 frame - -## 目标位置[相机系下：右方x为正，下方y为正，前方z为正] -float32[3] position - -## 目标姿态-欧拉角-(z,y,x) -float32[3] attitude - -## 目标姿态-四元数-(qx,qy,qz,qw) -float32[4] attitude_q - -## 视线角度[相机系下：右方x角度为正，下方y角度为正] -float32[2] sight_angle - -## 像素位置[相机系下：右方x为正，下方y为正] -int32[2] pixel_position - -## 偏航角误差 -float32 yaw_error - -## 类别 -int32 category diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/DetectionInfoSub.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/DetectionInfoSub.msg deleted file mode 100644 index 779a4bcd..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/DetectionInfoSub.msg +++ /dev/null @@ -1,9 +0,0 @@ -#目标框的位置（主要斜对角两个点） -float32 left -float32 top -float32 bot -float32 right - - -## TRACK TARGET(目标框ID) -int32 trackIds diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/FormationAssign.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/FormationAssign.msg deleted file mode 100644 index 64f7ccd3..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/FormationAssign.msg +++ /dev/null @@ -1,5 +0,0 @@ -#队形位置 -geometry_msgs/Point[] formation_poses - -#位置点是否选取 -bool[] assigned \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/GPSData.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/GPSData.msg deleted file mode 100644 index 48cba1d6..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/GPSData.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 latitude -float64 longitude -float64 altitude \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/GimbalControl.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/GimbalControl.msg deleted file mode 100644 index 0b32855a..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/GimbalControl.msg +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/GimbalState.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/GimbalState.msg deleted file mode 100644 index 1a579d2f..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/GimbalState.msg +++ /dev/null @@ -1,40 +0,0 @@ -Header header -uint8 Id - -# 0: 发一句,回一句: 此状态下，相机倍数zoomVal有效，imuAngleVel为估算直 -# 1: 发一句,一直回复: 此状态下，相机倍数zoomVal失效，imuAngleVel为真直 -uint8 feedbackMode - -#mode -# 0: 手动控制 -# 1: home -# 2: tracking -# 3: yaw follow 吊舱跟随无人机移动 -# 4: hold 吊舱不跟随无人机 -# 5: search 自动移动旋转 -uint8 mode - -# 是否视频录制 -bool isRecording - -# 是否开启自动缩放(VisionDiff需要指定面积才能生效) -# 0: 保持 -# 1: 放大 -# 2: 缩小 -# 3: 自动 -uint8 zoomState - -# 当前所处倍数 -float32 zoomVal - -#roll,pitch,yaw -float32[3] imuAngle - -#Current gimbal joint angles(roll,pitch,yaw), published at 30 Hz. -float32[3] rotorAngle - -# rpy_vel 角速度 -float32[3] imuAngleVel - -# rpy_tgt 目标角度 -float32[3] rotorAngleTarget \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/GlobalAruco.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/GlobalAruco.msg deleted file mode 100644 index 328af1b1..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/GlobalAruco.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiArucoInfo.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiArucoInfo.msg deleted file mode 100644 index 47310801..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiArucoInfo.msg +++ /dev/null @@ -1,7 +0,0 @@ -std_msgs/Header header - -## 检测到的aruco码数量 -int32 num_arucos - -## 每个aruco码的检测结果 -ArucoInfo[] aruco_infos diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiDetectionInfo.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiDetectionInfo.msg deleted file mode 100644 index 640d91f2..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiDetectionInfo.msg +++ /dev/null @@ -1,10 +0,0 @@ -Header header - -## 检测到的目标数量 -int32 num_objs - -## Detecting or Tracking (0:detect, 1:track) -int32 detect_or_track - -## 每个目标的检测结果 -DetectionInfo[] detection_infos diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiDetectionInfoSub.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiDetectionInfoSub.msg deleted file mode 100644 index bc9cd466..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiDetectionInfoSub.msg +++ /dev/null @@ -1,10 +0,0 @@ -std_msgs/Header header - -#模式：0：空闲 2.simaRPN 3.deepsort/sort -uint8 mode - -## 检测到的目标数量 -int32 num_objs - -## 每个目标的检测结果 -DetectionInfoSub[] detection_infos diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiUAVState.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiUAVState.msg deleted file mode 100644 index 81b2762e..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/MultiUAVState.msg +++ /dev/null @@ -1,7 +0,0 @@ -std_msgs/Header header - -## -int32 uav_num - -## 从1开始 -UAVState[] uav_state_all diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/OffsetPose.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/OffsetPose.msg deleted file mode 100644 index 776467e3..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/OffsetPose.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint8 uav_id -float32 x -float32 y \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaCommunication.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaCommunication.msg deleted file mode 100644 index dff2f07e..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaCommunication.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaGPS.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaGPS.msg deleted file mode 100644 index 48cba1d6..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaGPS.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 latitude -float64 longitude -float64 altitude \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaState.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaState.msg deleted file mode 100644 index 26cf3bb5..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/RheaState.msg +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/SwarmCommand.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/SwarmCommand.msg deleted file mode 100644 index 0182f750..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/SwarmCommand.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/TextInfo.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/TextInfo.msg deleted file mode 100644 index 25035901..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/TextInfo.msg +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVCommand.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVCommand.msg deleted file mode 100644 index 7bc358ca..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVCommand.msg +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVControlState.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVControlState.msg deleted file mode 100644 index 02b0a369..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVControlState.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVSetup.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVSetup.msg deleted file mode 100644 index 982d6fd0..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVSetup.msg +++ /dev/null @@ -1,17 +0,0 @@ -std_msgs/Header header - -## 无人机Setup类型（可用于模拟遥控器） -uint8 cmd -uint8 ARMING = 0 -uint8 SET_PX4_MODE = 1 -uint8 REBOOT_PX4 = 2 -uint8 SET_CONTROL_MODE = 3 - -bool arming - -## PX4模式名查询:http://wiki.ros.org/mavros/CustomModes -## 常用模式名:OFFBOARD,AUTO.LAND,AUTO.RTL,POSCTL -string px4_mode - -## INIT,MANUAL_CONTROL,HOVER_CONTROL,COMMAND_CONTROL,LAND_CONTROL -string control_state diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVState.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVState.msg deleted file mode 100644 index 1974c4f1..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/UAVState.msg +++ /dev/null @@ -1,59 +0,0 @@ -std_msgs/Header header - -## 无人机编号 -uint8 uav_id - -## 机载电脑是否连接上飞控，true已连接，false则不是 -bool connected -## 是否解锁，true为已解锁，false则不是 -bool armed -## PX4飞控当前飞行模式 int8 -string mode -## 无人机定位来源 -uint8 location_source -## 定位来源枚举 -uint8 MOCAP=0 -uint8 T265=1 -uint8 GAZEBO=2 -uint8 FAKE_ODOM=3 -uint8 GPS = 4 -uint8 RTK = 5 -uint8 UWB = 6 - -## odom标志位 -bool odom_valid - -## GPS状态,变量对应状态可参考mavros_msgs/GPSRAW中的fix_type -uint8 gps_status -## GPS状态枚举 -uint8 GPS_FIX_TYPE_NO_GPS=0 -uint8 GPS_FIX_TYPE_NO_FIX=1 -uint8 GPS_FIX_TYPE_2D_FIX=2 -uint8 GPS_FIX_TYPE_3D_FIX=3 -uint8 GPS_FIX_TYPE_DGPS=4 -uint8 GPS_FIX_TYPE_RTK_FLOATR=5 -uint8 GPS_FIX_TYPE_RTK_FIXEDR=6 -uint8 GPS_FIX_TYPE_STATIC=7 -uint8 GPS_FIX_TYPE_PPP=8 - -## 无人机位置、速度、姿态 -float32[3] position ## [m] -## 无人机经度、纬度、高度 -float32 latitude -float32 longitude -float32 altitude -## 无人机速度、姿态 -float32[3] velocity ## [m/s] -## 无人机姿态（欧拉角、四元数） -float32[3] attitude ## [rad] -geometry_msgs/Quaternion attitude_q ## 四元数 -## 无人机姿态角速度 -float32[3] attitude_rate ## [rad/s] -## 无人机电池状态 -float32 battery_state ## [V] -float32 battery_percetage ## [0-1] - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/VisionDiff.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/VisionDiff.msg deleted file mode 100644 index d47e1aca..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/VisionDiff.msg +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/msg/WindowPosition.msg b/src/无人机端代码/Modules/common/prometheus_msgs/msg/WindowPosition.msg deleted file mode 100644 index 8b4bcb4b..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/msg/WindowPosition.msg +++ /dev/null @@ -1,20 +0,0 @@ -std_msgs/Header header - -#模式：0：空闲 1.kcf 3.deepsort/sort(点击的id) -uint8 mode - - -#波门位置X,#波门位置Y(kcf,点击画面的功能的时候使用),左上角为（0,0） -int16 origin_x -int16 origin_y -int16 width -int16 height - -#波门位置X,#波门位置Y -#int16 window_position_x = origin_x + width/2 -#int16 window_position_y = origin_y + height/2 -int16 window_position_x -int16 window_position_y - -#算法检测结果的ID -int32 track_id diff --git a/src/无人机端代码/Modules/common/prometheus_msgs/package.xml b/src/无人机端代码/Modules/common/prometheus_msgs/package.xml deleted file mode 100644 index d9695c65..00000000 --- a/src/无人机端代码/Modules/common/prometheus_msgs/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - prometheus_msgs - 0.0.0 - The prometheus_msgs package - - Yuhua Qi - - TODO - - message_generation - message_runtime - catkin - std_msgs - actionlib_msgs - std_msgs - actionlib_msgs - std_msgs - actionlib_msgs - geometry_msgs - geometry_msgs - geometry_msgs - sensor_msgs - sensor_msgs - sensor_msgs - - - - - diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/CMakeLists.txt b/src/无人机端代码/Modules/common/quadrotor_msgs/CMakeLists.txt deleted file mode 100644 index f82385d3..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/CMakeLists.txt +++ /dev/null @@ -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}) - - diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/AuxCommand.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/AuxCommand.msg deleted file mode 100644 index f59bf356..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/AuxCommand.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Corrections.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Corrections.msg deleted file mode 100644 index e0f4e888..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Corrections.msg +++ /dev/null @@ -1,2 +0,0 @@ -float64 kf_correction -float64[2] angle_corrections diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Gains.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Gains.msg deleted file mode 100644 index f5d10a33..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Gains.msg +++ /dev/null @@ -1,4 +0,0 @@ -float64 Kp -float64 Kd -float64 Kp_yaw -float64 Kd_yaw diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/LQRTrajectory.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/LQRTrajectory.msg deleted file mode 100644 index 0a34e9b6..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/LQRTrajectory.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Odometry.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Odometry.msg deleted file mode 100644 index 3272d71a..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Odometry.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/OutputData.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/OutputData.msg deleted file mode 100644 index ac958880..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/OutputData.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PPROutputData.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PPROutputData.msg deleted file mode 100644 index 70434a02..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PPROutputData.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PolynomialTrajectory.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PolynomialTrajectory.msg deleted file mode 100644 index 0aab297a..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PolynomialTrajectory.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PositionCommand.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PositionCommand.msg deleted file mode 100644 index 49c6fa1d..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/PositionCommand.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Px4ctrlDebug.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Px4ctrlDebug.msg deleted file mode 100644 index f28ba2c3..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Px4ctrlDebug.msg +++ /dev/null @@ -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 - diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/SO3Command.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/SO3Command.msg deleted file mode 100644 index d3868efb..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/SO3Command.msg +++ /dev/null @@ -1,6 +0,0 @@ -Header header -geometry_msgs/Vector3 force -geometry_msgs/Quaternion orientation -float64[3] kR -float64[3] kOm -quadrotor_msgs/AuxCommand aux diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Serial.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Serial.msg deleted file mode 100644 index 5a54cce3..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/Serial.msg +++ /dev/null @@ -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 diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/StatusData.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/StatusData.msg deleted file mode 100644 index d176e4f0..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/StatusData.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header -uint16 loop_rate -float64 voltage -uint8 seq diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/TRPYCommand.msg b/src/无人机端代码/Modules/common/quadrotor_msgs/msg/TRPYCommand.msg deleted file mode 100644 index 0d471a62..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/msg/TRPYCommand.msg +++ /dev/null @@ -1,6 +0,0 @@ -Header header -float32 thrust -float32 roll -float32 pitch -float32 yaw -quadrotor_msgs/AuxCommand aux diff --git a/src/无人机端代码/Modules/common/quadrotor_msgs/package.xml b/src/无人机端代码/Modules/common/quadrotor_msgs/package.xml deleted file mode 100644 index 42e9f76e..00000000 --- a/src/无人机端代码/Modules/common/quadrotor_msgs/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - quadrotor_msgs - 0.0.0 - quadrotor_msgs - Kartik Mohta - http://ros.org/wiki/quadrotor_msgs - BSD - catkin - geometry_msgs - nav_msgs - message_generation - geometry_msgs - nav_msgs - message_runtime - - - - - diff --git a/src/无人机端代码/Modules/communication/CMakeLists.txt b/src/无人机端代码/Modules/communication/CMakeLists.txt deleted file mode 100644 index e702198b..00000000 --- a/src/无人机端代码/Modules/communication/CMakeLists.txt +++ /dev/null @@ -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) - diff --git a/src/无人机端代码/Modules/communication/include/autonomous_landing_topic.hpp b/src/无人机端代码/Modules/communication/include/autonomous_landing_topic.hpp deleted file mode 100644 index cd7934ec..00000000 --- a/src/无人机端代码/Modules/communication/include/autonomous_landing_topic.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef AUTONOMOUS_LANDING_TOPIC_HPP -#define AUTONOMOUS_LANDING_TOPIC_HPP - -#include -#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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/include/communication_bridge.hpp b/src/无人机端代码/Modules/communication/include/communication_bridge.hpp deleted file mode 100644 index fcf56d1b..00000000 --- a/src/无人机端代码/Modules/communication/include/communication_bridge.hpp +++ /dev/null @@ -1,93 +0,0 @@ -#ifndef COMUNICATION_BRIDGE_HPP -#define COMUNICATION_BRIDGE_HPP - -#include -#include "communication.hpp" - -#include - -#include -#include "Struct.hpp" - -#include "uav_basic_topic.hpp" -#include "ugv_basic_topic.hpp" -#include "swarm_control_topic.hpp" -#include "autonomous_landing_topic.hpp" -#include "gimbal_basic_topic.hpp" -#include "object_tracking_topic.hpp" - -#include -#include - -enum UserType -{ - UAV = 1, - UGV = 2 -}; - -class CommunicationBridge : public Communication -{ -public: - CommunicationBridge(ros::NodeHandle &nh); - - ~CommunicationBridge(); - - void serverFun(); - - //根据协议中MSG_ID的值，将数据段数据转化为正确的结构体 - void pubMsg(int msg_id); - - void recvData(struct UAVState uav_state); - void recvData(struct UAVCommand uav_cmd); - void recvData(struct SwarmCommand swarm_command); - void recvData(struct ConnectState connect_state); - void recvData(struct GimbalControl gimbal_control); - void recvData(struct GimbalService gimbal_service); - void recvData(struct GimbalParamSet param_set); - void recvData(struct WindowPosition window_position); - void recvData(struct RheaControl rhea_control); - void recvData(struct RheaState rhea_state); - void recvData(struct ImageData image_data); - void recvData(struct UAVSetup uav_setup); - void recvData(struct ModeSelection mode_selection); - - void modeSwitch(struct ModeSelection mode_selection); - - //接收组播地址的数据 - void multicastUdpFun(); - - //给地面站发送心跳包 - void toGroundStationFun(); - - void init(); - //ros::NodeHandle nh; - - void createImage(struct ImageData image_data); - - bool createMode(struct ModeSelection mode_selection); - - bool deleteMode(struct ModeSelection mode_selection); - -private: - //std::shared_ptr swarm_control_ ; - SwarmControl *swarm_control_ = NULL; - UGVBasic *ugv_basic_ = NULL; - UAVBasic *uav_basic_ = NULL; - //std::vector swarm_control_simulation_; - std::map swarm_control_simulation_; - AutonomousLanding *autonomous_landing_ = NULL; - GimbalBasic *gimbal_basic_ = NULL; - ObjectTracking *object_tracking_ = NULL; - - int current_mode_ = 0; - - int is_simulation_, swarm_num_, swarm_data_update_timeout_; - ros::NodeHandle nh_; - - bool is_heartbeat_ready_ = false; - - int user_type_; - -}; - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/include/gimbal_basic_topic.hpp b/src/无人机端代码/Modules/communication/include/gimbal_basic_topic.hpp deleted file mode 100644 index cf7b835d..00000000 --- a/src/无人机端代码/Modules/communication/include/gimbal_basic_topic.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef GimbalBasic_HPP -#define GimbalBasic_HPP - -#include -#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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/include/object_tracking_topic.hpp b/src/无人机端代码/Modules/communication/include/object_tracking_topic.hpp deleted file mode 100644 index 901c911e..00000000 --- a/src/无人机端代码/Modules/communication/include/object_tracking_topic.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef OBJECT_TRACKING_TOPIC_HPP -#define OBJECT_TRACKING_TOPIC_HPP - -#include -#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 diff --git a/src/无人机端代码/Modules/communication/include/swarm_control_topic.hpp b/src/无人机端代码/Modules/communication/include/swarm_control_topic.hpp deleted file mode 100644 index 04ba9960..00000000 --- a/src/无人机端代码/Modules/communication/include/swarm_control_topic.hpp +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef SWARM_CONTROL_TOPIC_HPP -#define SWARM_CONTROL_TOPIC_HPP - -#include -#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 - -using namespace std; - -//订阅: /prometheus/formation_assign/result -//发布: /Prometheus/formation_assign/info - -struct MultiUAVState -{ - int uav_num; - std::vector 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 simulation_communication_state_pub; - - bool is_simulation_; - - std::string udp_ip, multicast_udp_ip; - - std::string user_type_ = ""; -}; - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/include/uav_basic_topic.hpp b/src/无人机端代码/Modules/communication/include/uav_basic_topic.hpp deleted file mode 100644 index 203c4568..00000000 --- a/src/无人机端代码/Modules/communication/include/uav_basic_topic.hpp +++ /dev/null @@ -1,75 +0,0 @@ -#ifndef UAV_BASIC_TOPIC_HPP -#define UAV_BASIC_TOPIC_HPP - -#include -#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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/include/ugv_basic_topic.hpp b/src/无人机端代码/Modules/communication/include/ugv_basic_topic.hpp deleted file mode 100644 index 2a2fdf32..00000000 --- a/src/无人机端代码/Modules/communication/include/ugv_basic_topic.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef UGV_BASIC_TOPIC_HPP -#define UGV_BASIC_TOPIC_HPP - -#include -#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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/launch/bridge.launch b/src/无人机端代码/Modules/communication/launch/bridge.launch deleted file mode 100644 index 574ef1ca..00000000 --- a/src/无人机端代码/Modules/communication/launch/bridge.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/communication/package.xml b/src/无人机端代码/Modules/communication/package.xml deleted file mode 100644 index 624633b5..00000000 --- a/src/无人机端代码/Modules/communication/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - prometheus_communication_bridge - 0.0.0 - The ground_station_bridge module - - Amov - Aomv - - catkin - roscpp - std_msgs - std_srvs - geometry_msgs - roscpp - std_msgs - std_srvs - geometry_msgs - roscpp - std_msgs - std_srvs - geometry_msgs - sensor_msgs - sensor_msgs - sensor_msgs - mavros_msgs - mavros_msgs - mavros_msgs - message_runtime - - - - - diff --git a/src/无人机端代码/Modules/communication/shard/include/CRC.h b/src/无人机端代码/Modules/communication/shard/include/CRC.h deleted file mode 100644 index e20db354..00000000 --- a/src/无人机端代码/Modules/communication/shard/include/CRC.h +++ /dev/null @@ -1,2066 +0,0 @@ -/** - @file CRC.h - @author Daniel Bahr - @version 1.1.0.0 - @copyright - @parblock - CRC++ - Copyright (c) 2021, Daniel Bahr - 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 CRC++ 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. - @endparblock -*/ - -/* - CRC++ can be configured by setting various #defines before #including this header file: - - #define crcpp_uint8 - Specifies the type used to store CRCs that have a width of 8 bits or less. - This type is not used in CRC calculations. Defaults to ::std::uint8_t. - #define crcpp_uint16 - Specifies the type used to store CRCs that have a width between 9 and 16 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint16_t. - #define crcpp_uint32 - Specifies the type used to store CRCs that have a width between 17 and 32 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint32_t. - #define crcpp_uint64 - Specifies the type used to store CRCs that have a width between 33 and 64 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint64_t. - #define crcpp_size - This type is used for loop iteration and function signatures only. Defaults to ::std::size_t. - #define CRCPP_USE_NAMESPACE - Define to place all CRC++ code within the ::CRCPP namespace. - #define CRCPP_BRANCHLESS - Define to enable a branchless CRC implementation. The branchless implementation uses a single integer - multiplication in the bit-by-bit calculation instead of a small conditional. The branchless implementation - may be faster on processor architectures which support single-instruction integer multiplication. - #define CRCPP_USE_CPP11 - Define to enables C++11 features (move semantics, constexpr, static_assert, etc.). - #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. -*/ - -#ifndef CRCPP_CRC_H_ -#define CRCPP_CRC_H_ - -#include // Includes CHAR_BIT -#ifdef CRCPP_USE_CPP11 -#include // Includes ::std::size_t -#include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t -#else -#include // Includes size_t -#include // Includes uint8_t, uint16_t, uint32_t, uint64_t -#endif -#include // Includes ::std::numeric_limits -#include // Includes ::std::move - -#ifndef crcpp_uint8 -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint8 ::std::uint8_t -# else - /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint8 uint8_t -# endif -#endif - -#ifndef crcpp_uint16 -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint16 ::std::uint16_t -# else - /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint16 uint16_t -# endif -#endif - -#ifndef crcpp_uint32 -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint32 ::std::uint32_t -# else - /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint32 uint32_t -# endif -#endif - -#ifndef crcpp_uint64 -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint64 ::std::uint64_t -# else - /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint64 uint64_t -# endif -#endif - -#ifndef crcpp_size -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned size definition, used for specifying data sizes. -# define crcpp_size ::std::size_t -# else - /// @brief Unsigned size definition, used for specifying data sizes. -# define crcpp_size size_t -# endif -#endif - -#ifdef CRCPP_USE_CPP11 - /// @brief Compile-time expression definition. -# define crcpp_constexpr constexpr -#else - /// @brief Compile-time expression definition. -# define crcpp_constexpr const -#endif - -#ifdef CRCPP_USE_NAMESPACE -namespace CRCPP -{ -#endif - -/** - @brief Static class for computing CRCs. - @note This class supports computation of full and multi-part CRCs, using a bit-by-bit algorithm or a - byte-by-byte lookup table. The CRCs are calculated using as many optimizations as is reasonable. - If compiling with C++11, the constexpr keyword is used liberally so that many calculations are - performed at compile-time instead of at runtime. -*/ -class CRC -{ -public: - // Forward declaration - template - struct Table; - - /** - @brief CRC parameters. - */ - template - struct Parameters - { - CRCType polynomial; ///< CRC polynomial - CRCType initialValue; ///< Initial CRC value - CRCType finalXOR; ///< Value to XOR with the final CRC - bool reflectInput; ///< true to reflect all input bytes - bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) - - Table MakeTable() const; - }; - - /** - @brief CRC lookup table. After construction, the CRC parameters are fixed. - @note A CRC table can be used for multiple CRC calculations. - */ - template - struct Table - { - // Constructors are intentionally NOT marked explicit. - Table(const Parameters & parameters); - -#ifdef CRCPP_USE_CPP11 - Table(Parameters && parameters); -#endif - - const Parameters & GetParameters() const; - - const CRCType * GetTable() const; - - CRCType operator[](unsigned char index) const; - - private: - void InitTable(); - - Parameters parameters; ///< CRC parameters used to construct the table - CRCType table[1 << CHAR_BIT]; ///< CRC lookup table - }; - - // The number of bits in CRCType must be at least as large as CRCWidth. - // CRCType must be an unsigned integer type or a custom type with operator overloads. - template - static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters); - - template - static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); - - template - static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable); - - template - static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); - - template - static CRCType CalculateBits(const void * data, crcpp_size size, const Parameters & parameters); - - template - static CRCType CalculateBits(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); - - template - static CRCType CalculateBits(const void * data, crcpp_size size, const Table & lookupTable); - - template - static CRCType CalculateBits(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); - - // Common CRCs up to 64 bits. - // Note: Check values are the computed CRCs when given an ASCII input of "123456789" (without null terminator) -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters< crcpp_uint8, 4> & CRC_4_ITU(); - static const Parameters< crcpp_uint8, 5> & CRC_5_EPC(); - static const Parameters< crcpp_uint8, 5> & CRC_5_ITU(); - static const Parameters< crcpp_uint8, 5> & CRC_5_USB(); - static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000A(); - static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000B(); - static const Parameters< crcpp_uint8, 6> & CRC_6_ITU(); - static const Parameters< crcpp_uint8, 6> & CRC_6_NR(); - static const Parameters< crcpp_uint8, 7> & CRC_7(); -#endif - static const Parameters< crcpp_uint8, 8> & CRC_8(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters< crcpp_uint8, 8> & CRC_8_EBU(); - static const Parameters< crcpp_uint8, 8> & CRC_8_MAXIM(); - static const Parameters< crcpp_uint8, 8> & CRC_8_WCDMA(); - static const Parameters< crcpp_uint8, 8> & CRC_8_LTE(); - static const Parameters & CRC_10(); - static const Parameters & CRC_10_CDMA2000(); - static const Parameters & CRC_11(); - static const Parameters & CRC_11_NR(); - static const Parameters & CRC_12_CDMA2000(); - static const Parameters & CRC_12_DECT(); - static const Parameters & CRC_12_UMTS(); - static const Parameters & CRC_13_BBC(); - static const Parameters & CRC_15(); - static const Parameters & CRC_15_MPT1327(); -#endif - static const Parameters & CRC_16_ARC(); - static const Parameters & CRC_16_BUYPASS(); - static const Parameters & CRC_16_CCITTFALSE(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_16_CDMA2000(); - static const Parameters & CRC_16_CMS(); - static const Parameters & CRC_16_DECTR(); - static const Parameters & CRC_16_DECTX(); - static const Parameters & CRC_16_DNP(); -#endif - static const Parameters & CRC_16_GENIBUS(); - static const Parameters & CRC_16_KERMIT(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_16_MAXIM(); - static const Parameters & CRC_16_MODBUS(); - static const Parameters & CRC_16_T10DIF(); - static const Parameters & CRC_16_USB(); -#endif - static const Parameters & CRC_16_X25(); - static const Parameters & CRC_16_XMODEM(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_17_CAN(); - static const Parameters & CRC_21_CAN(); - static const Parameters & CRC_24(); - static const Parameters & CRC_24_FLEXRAYA(); - static const Parameters & CRC_24_FLEXRAYB(); - static const Parameters & CRC_24_LTEA(); - static const Parameters & CRC_24_LTEB(); - static const Parameters & CRC_24_NRC(); - static const Parameters & CRC_30(); -#endif - static const Parameters & CRC_32(); - static const Parameters & CRC_32_BZIP2(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_C(); -#endif - static const Parameters & CRC_32_MPEG2(); - static const Parameters & CRC_32_POSIX(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_Q(); - static const Parameters & CRC_40_GSM(); - static const Parameters & CRC_64(); -#endif - -#ifdef CRCPP_USE_CPP11 - CRC() = delete; - CRC(const CRC & other) = delete; - CRC & operator=(const CRC & other) = delete; - CRC(CRC && other) = delete; - CRC & operator=(CRC && other) = delete; -#endif - -private: -#ifndef CRCPP_USE_CPP11 - CRC(); - CRC(const CRC & other); - CRC & operator=(const CRC & other); -#endif - - template - static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); - - template - static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); - - template - static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); - - template - static CRCType CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); - - template - static CRCType CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); - - template - static CRCType CalculateRemainderBits(unsigned char byte, crcpp_size numBits, const Parameters & parameters, CRCType remainder); -}; - -/** - @brief Returns a CRC lookup table construct using these CRC parameters. - @note This function primarily exists to allow use of the auto keyword instead of instantiating - a table directly, since template parameters are not inferred in constructors. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC lookup table -*/ -template -inline CRC::Table CRC::Parameters::MakeTable() const -{ - // This should take advantage of RVO and optimize out the copy. - return CRC::Table(*this); -} - -/** - @brief Constructs a CRC table from a set of CRC parameters - @param[in] params CRC parameters - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC -*/ -template -inline CRC::Table::Table(const Parameters & params) : - parameters(params) -{ - InitTable(); -} - -#ifdef CRCPP_USE_CPP11 -/** - @brief Constructs a CRC table from a set of CRC parameters - @param[in] params CRC parameters - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC -*/ -template -inline CRC::Table::Table(Parameters && params) : - parameters(::std::move(params)) -{ - InitTable(); -} -#endif - -/** - @brief Gets the CRC parameters used to construct the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC parameters -*/ -template -inline const CRC::Parameters & CRC::Table::GetParameters() const -{ - return parameters; -} - -/** - @brief Gets the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC table -*/ -template -inline const CRCType * CRC::Table::GetTable() const -{ - return table; -} - -/** - @brief Gets an entry in the CRC table - @param[in] index Index into the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC table entry -*/ -template -inline CRCType CRC::Table::operator[](unsigned char index) const -{ - return table[index]; -} - -/** - @brief Initializes a CRC table. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC -*/ -template -inline void CRC::Table::InitTable() -{ - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); - - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - - CRCType crc; - unsigned char byte = 0; - - // Loop over each dividend (each possible number storable in an unsigned char) - do - { - crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); - - // This mask might not be necessary; all unit tests pass with this line commented out, - // but that might just be a coincidence based on the CRC parameters used for testing. - // In any case, this is harmless to leave in and only adds a single machine instruction per loop iteration. - crc &= BIT_MASK; - - if (!parameters.reflectInput && CRCWidth < CHAR_BIT) - { - // Undo the special operation at the end of the CalculateRemainder() - // function for non-reflected CRCs < CHAR_BIT. - crc = static_cast(crc << SHIFT); - } - - table[byte] = crc; - } - while (++byte); -} - -/** - @brief Computes a CRC. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data, in bytes - @param[in] parameters CRC parameters - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters) -{ - CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} -/** - @brief Appends additional data to a previous CRC calculation. - @note This function can be used to compute multi-part CRCs. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data, in bytes - @param[in] parameters CRC parameters - @param[in] crc CRC from a previous calculation - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) -{ - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); - - remainder = CalculateRemainder(data, size, parameters, remainder); - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Computes a CRC via a lookup table. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data, in bytes - @param[in] lookupTable CRC lookup table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable) -{ - const Parameters & parameters = lookupTable.GetParameters(); - - CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Appends additional data to a previous CRC calculation using a lookup table. - @note This function can be used to compute multi-part CRCs. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data, in bytes - @param[in] lookupTable CRC lookup table - @param[in] crc CRC from a previous calculation - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) -{ - const Parameters & parameters = lookupTable.GetParameters(); - - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); - - remainder = CalculateRemainder(data, size, lookupTable, remainder); - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Computes a CRC. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data, in bits - @param[in] parameters CRC parameters - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::CalculateBits(const void * data, crcpp_size size, const Parameters & parameters) -{ - CRCType remainder = parameters.initialValue; - - // Calculate the remainder on a whole number of bytes first, then call - // a special-case function for the remaining bits. - crcpp_size wholeNumberOfBytes = size / CHAR_BIT; - if (wholeNumberOfBytes > 0) - { - remainder = CalculateRemainder(data, wholeNumberOfBytes, parameters, remainder); - } - - crcpp_size remainingNumberOfBits = size % CHAR_BIT; - if (remainingNumberOfBits != 0) - { - unsigned char lastByte = *(reinterpret_cast(data) + wholeNumberOfBytes); - remainder = CalculateRemainderBits(lastByte, remainingNumberOfBits, parameters, remainder); - } - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} -/** - @brief Appends additional data to a previous CRC calculation. - @note This function can be used to compute multi-part CRCs. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data, in bits - @param[in] parameters CRC parameters - @param[in] crc CRC from a previous calculation - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::CalculateBits(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) -{ - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); - - // Calculate the remainder on a whole number of bytes first, then call - // a special-case function for the remaining bits. - crcpp_size wholeNumberOfBytes = size / CHAR_BIT; - if (wholeNumberOfBytes > 0) - { - remainder = CalculateRemainder(data, wholeNumberOfBytes, parameters, parameters.initialValue); - } - - crcpp_size remainingNumberOfBits = size % CHAR_BIT; - if (remainingNumberOfBits != 0) - { - unsigned char lastByte = *(reinterpret_cast(data) + wholeNumberOfBytes); - remainder = CalculateRemainderBits(lastByte, remainingNumberOfBits, parameters, remainder); - } - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Computes a CRC via a lookup table. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data, in bits - @param[in] lookupTable CRC lookup table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::CalculateBits(const void * data, crcpp_size size, const Table & lookupTable) -{ - const Parameters & parameters = lookupTable.GetParameters(); - - CRCType remainder = parameters.initialValue; - - // Calculate the remainder on a whole number of bytes first, then call - // a special-case function for the remaining bits. - crcpp_size wholeNumberOfBytes = size / CHAR_BIT; - if (wholeNumberOfBytes > 0) - { - remainder = CalculateRemainder(data, wholeNumberOfBytes, lookupTable, remainder); - } - - crcpp_size remainingNumberOfBits = size % CHAR_BIT; - if (remainingNumberOfBits != 0) - { - unsigned char lastByte = *(reinterpret_cast(data) + wholeNumberOfBytes); - remainder = CalculateRemainderBits(lastByte, remainingNumberOfBits, parameters, remainder); - } - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Appends additional data to a previous CRC calculation using a lookup table. - @note This function can be used to compute multi-part CRCs. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data, in bits - @param[in] lookupTable CRC lookup table - @param[in] crc CRC from a previous calculation - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::CalculateBits(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) -{ - const Parameters & parameters = lookupTable.GetParameters(); - - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); - - // Calculate the remainder on a whole number of bytes first, then call - // a special-case function for the remaining bits. - crcpp_size wholeNumberOfBytes = size / CHAR_BIT; - if (wholeNumberOfBytes > 0) - { - remainder = CalculateRemainder(data, wholeNumberOfBytes, lookupTable, parameters.initialValue); - } - - crcpp_size remainingNumberOfBits = size % CHAR_BIT; - if (remainingNumberOfBits > 0) - { - unsigned char lastByte = *(reinterpret_cast(data) + wholeNumberOfBytes); - remainder = CalculateRemainderBits(lastByte, remainingNumberOfBits, parameters, remainder); - } - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Reflects (i.e. reverses the bits within) an integer value. - @param[in] value Value to reflect - @param[in] numBits Number of bits in the integer which will be reflected - @tparam IntegerType Integer type of the value being reflected - @return Reflected value -*/ -template -inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) -{ - IntegerType reversedValue(0); - - for (crcpp_uint16 i = 0; i < numBits; ++i) - { - reversedValue = static_cast((reversedValue << 1) | (value & 1)); - value = static_cast(value >> 1); - } - - return reversedValue; -} - -/** - @brief Computes the final reflection and XOR of a CRC remainder. - @param[in] remainder CRC remainder to reflect and XOR - @param[in] finalXOR Final value to XOR with the remainder - @param[in] reflectOutput true to reflect each byte of the remainder before the XOR - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return Final CRC -*/ -template -inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput) -{ - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); - - if (reflectOutput) - { - remainder = Reflect(remainder, CRCWidth); - } - - return (remainder ^ finalXOR) & BIT_MASK; -} - -/** - @brief Undoes the process of computing the final reflection and XOR of a CRC remainder. - @note This function allows for computation of multi-part CRCs - @note Calling UndoFinalize() followed by Finalize() (or vice versa) will always return the original remainder value: - - CRCType x = ...; - CRCType y = Finalize(x, finalXOR, reflectOutput); - CRCType z = UndoFinalize(y, finalXOR, reflectOutput); - assert(x == z); - - @param[in] crc Reflected and XORed CRC - @param[in] finalXOR Final value XORed with the remainder - @param[in] reflectOutput true if the remainder is to be reflected - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return Un-finalized CRC remainder -*/ -template -inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutput) -{ - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); - - crc = (crc & BIT_MASK) ^ finalXOR; - - if (reflectOutput) - { - crc = Reflect(crc, CRCWidth); - } - - return crc; -} - -/** - @brief Computes a CRC remainder. - @param[in] data Data over which the remainder will be computed - @param[in] size Size of the data, in bytes - @param[in] parameters CRC parameters - @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC remainder -*/ -template -inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) -{ -#ifdef CRCPP_USE_CPP11 - // This static_assert is put here because this function will always be compiled in no matter what - // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. - static_assert(::std::numeric_limits::digits >= CRCWidth, "CRCType is too small to contain a CRC of width CRCWidth."); -#else - // Catching this compile-time error is very important. Sadly, the compiler error will be very cryptic, but it's - // better than nothing. - enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; -#endif - - const unsigned char * current = reinterpret_cast(data); - - // Slightly different implementations based on the parameters. The current implementations try to eliminate as much - // computation from the inner loop (looping over each bit) as possible. - if (parameters.reflectInput) - { - CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); - while (size--) - { - remainder = static_cast(remainder ^ *current++); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & 1) - // remainder = (remainder >> 1) ^ polynomial; - // else - // remainder >>= 1; - remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); -#else - remainder = static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1)); -#endif - } - } - } - else if (CRCWidth >= CHAR_BIT) - { - static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); -#ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); -#endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - - while (size--) - { - remainder = static_cast(remainder ^ (static_cast(*current++) << SHIFT)); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CRC_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ parameters.polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); -#else - remainder = static_cast((remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); -#endif - } - } - } - else - { - static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); -#ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); -#endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - - CRCType polynomial = static_cast(parameters.polynomial << SHIFT); - remainder = static_cast(remainder << SHIFT); - - while (size--) - { - remainder = static_cast(remainder ^ *current++); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); -#else - remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); -#endif - } - } - - remainder = static_cast(remainder >> SHIFT); - } - - return remainder; -} - -/** - @brief Computes a CRC remainder using lookup table. - @param[in] data Data over which the remainder will be computed - @param[in] size Size of the data, in bytes - @param[in] lookupTable CRC lookup table - @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC remainder -*/ -template -inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) -{ - const unsigned char * current = reinterpret_cast(data); - - if (lookupTable.GetParameters().reflectInput) - { - while (size--) - { -#if defined(WIN32) || defined(_WIN32) || defined(WINCE) - // Disable warning about data loss when doing (remainder >> CHAR_BIT) when - // remainder is one byte long. The algorithm is still correct in this case, - // though it's possible that one additional machine instruction will be executed. -# pragma warning (push) -# pragma warning (disable : 4333) -#endif - remainder = static_cast((remainder >> CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]); -#if defined(WIN32) || defined(_WIN32) || defined(WINCE) -# pragma warning (pop) -#endif - } - } - else if (CRCWidth >= CHAR_BIT) - { - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - - while (size--) - { - remainder = static_cast((remainder << CHAR_BIT) ^ lookupTable[static_cast((remainder >> SHIFT) ^ *current++)]); - } - } - else - { - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - - remainder = static_cast(remainder << SHIFT); - - while (size--) - { - // Note: no need to mask here since remainder is guaranteed to fit in a single byte. - remainder = lookupTable[static_cast(remainder ^ *current++)]; - } - - remainder = static_cast(remainder >> SHIFT); - } - - return remainder; -} - -template -inline CRCType CRC::CalculateRemainderBits(unsigned char byte, crcpp_size numBits, const Parameters & parameters, CRCType remainder) -{ - // Slightly different implementations based on the parameters. The current implementations try to eliminate as much - // computation from the inner loop (looping over each bit) as possible. - if (parameters.reflectInput) - { - CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); - remainder = static_cast(remainder ^ byte); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < numBits; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & 1) - // remainder = (remainder >> 1) ^ polynomial; - // else - // remainder >>= 1; - remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); -#else - remainder = static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1)); -#endif - } - } - else if (CRCWidth >= CHAR_BIT) - { - static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); -#ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); -#endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - - remainder = static_cast(remainder ^ (static_cast(byte) << SHIFT)); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < numBits; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CRC_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ parameters.polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); -#else - remainder = static_cast((remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); -#endif - } - } - else - { - static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); -#ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); -#endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - - CRCType polynomial = static_cast(parameters.polynomial << SHIFT); - remainder = static_cast((remainder << SHIFT) ^ byte); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < numBits; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); -#else - remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); -#endif - } - - remainder = static_cast(remainder >> SHIFT); - } - - return remainder; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-4 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-4 ITU has the following parameters and check value: - - polynomial = 0x3 - - initial value = 0x0 - - final XOR = 0x0 - - reflect input = true - - reflect output = true - - check value = 0x7 - @return CRC-4 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_4_ITU() -{ - static const Parameters parameters = { 0x3, 0x0, 0x0, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 EPC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 EPC has the following parameters and check value: - - polynomial = 0x09 - - initial value = 0x09 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x00 - @return CRC-5 EPC parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_EPC() -{ - static const Parameters parameters = { 0x09, 0x09, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 ITU has the following parameters and check value: - - polynomial = 0x15 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x07 - @return CRC-5 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_ITU() -{ - static const Parameters parameters = { 0x15, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 USB. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 USB has the following parameters and check value: - - polynomial = 0x05 - - initial value = 0x1F - - final XOR = 0x1F - - reflect input = true - - reflect output = true - - check value = 0x19 - @return CRC-5 USB parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_USB() -{ - static const Parameters parameters = { 0x05, 0x1F, 0x1F, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 CDMA2000-A. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 CDMA2000-A has the following parameters and check value: - - polynomial = 0x27 - - initial value = 0x3F - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x0D - @return CRC-6 CDMA2000-A parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() -{ - static const Parameters parameters = { 0x27, 0x3F, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 CDMA2000-B. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 CDMA2000-A has the following parameters and check value: - - polynomial = 0x07 - - initial value = 0x3F - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x3B - @return CRC-6 CDMA2000-B parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() -{ - static const Parameters parameters = { 0x07, 0x3F, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 ITU has the following parameters and check value: - - polynomial = 0x03 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x06 - @return CRC-6 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_ITU() -{ - static const Parameters parameters = { 0x03, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 NR. - @note The parameters are static and are delayed-constructed to reduce memory - footprint. - @note CRC-6 NR has the following parameters and check value: - - polynomial = 0x21 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x15 - @return CRC-6 NR parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_NR() -{ - static const Parameters parameters = { 0x21, 0x00, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-7 JEDEC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-7 JEDEC has the following parameters and check value: - - polynomial = 0x09 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x75 - @return CRC-7 JEDEC parameters -*/ -inline const CRC::Parameters & CRC::CRC_7() -{ - static const Parameters parameters = { 0x09, 0x00, 0x00, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-8 SMBus. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 SMBus has the following parameters and check value: - - polynomial = 0x07 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0xF4 - @return CRC-8 SMBus parameters -*/ -inline const CRC::Parameters & CRC::CRC_8() -{ - static const Parameters parameters = { 0x07, 0x00, 0x00, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-8 EBU (aka CRC-8 AES). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 EBU has the following parameters and check value: - - polynomial = 0x1D - - initial value = 0xFF - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x97 - @return CRC-8 EBU parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_EBU() -{ - static const Parameters parameters = { 0x1D, 0xFF, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-8 MAXIM (aka CRC-8 DOW-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 MAXIM has the following parameters and check value: - - polynomial = 0x31 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0xA1 - @return CRC-8 MAXIM parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_MAXIM() -{ - static const Parameters parameters = { 0x31, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-8 WCDMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 WCDMA has the following parameters and check value: - - polynomial = 0x9B - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x25 - @return CRC-8 WCDMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_WCDMA() -{ - static const Parameters parameters = { 0x9B, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-8 LTE. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 LTE has the following parameters and check value: - - polynomial = 0x9B - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0xEA - @return CRC-8 LTE parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_LTE() -{ - static const Parameters parameters = { 0x9B, 0x00, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-10 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-10 ITU has the following parameters and check value: - - polynomial = 0x233 - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x199 - @return CRC-10 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_10() -{ - static const Parameters parameters = { 0x233, 0x000, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-10 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-10 CDMA2000 has the following parameters and check value: - - polynomial = 0x3D9 - - initial value = 0x3FF - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x233 - @return CRC-10 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_10_CDMA2000() -{ - static const Parameters parameters = { 0x3D9, 0x3FF, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-11 FlexRay. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-11 FlexRay has the following parameters and check value: - - polynomial = 0x385 - - initial value = 0x01A - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x5A3 - @return CRC-11 FlexRay parameters -*/ -inline const CRC::Parameters & CRC::CRC_11() -{ - static const Parameters parameters = { 0x385, 0x01A, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-11 NR. - @note The parameters are static and are delayed-constructed to reduce memory - footprint. - @note CRC-11 NR has the following parameters and check value: - - polynomial = 0x621 - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x5CA - @return CRC-11 NR parameters -*/ -inline const CRC::Parameters & CRC::CRC_11_NR() -{ - static const Parameters parameters = { 0x621, 0x000, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 CDMA2000 has the following parameters and check value: - - polynomial = 0xF13 - - initial value = 0xFFF - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0xD4D - @return CRC-12 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_CDMA2000() -{ - static const Parameters parameters = { 0xF13, 0xFFF, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 DECT (aka CRC-12 X-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 DECT has the following parameters and check value: - - polynomial = 0x80F - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0xF5B - @return CRC-12 DECT parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_DECT() -{ - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 UMTS (aka CRC-12 3GPP). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 UMTS has the following parameters and check value: - - polynomial = 0x80F - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = true - - check value = 0xDAF - @return CRC-12 UMTS parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_UMTS() -{ - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-13 BBC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-13 BBC has the following parameters and check value: - - polynomial = 0x1CF5 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x04FA - @return CRC-13 BBC parameters -*/ -inline const CRC::Parameters & CRC::CRC_13_BBC() -{ - static const Parameters parameters = { 0x1CF5, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-15 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-15 CAN has the following parameters and check value: - - polynomial = 0x4599 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x059E - @return CRC-15 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_15() -{ - static const Parameters parameters = { 0x4599, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-15 MPT1327. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-15 MPT1327 has the following parameters and check value: - - polynomial = 0x6815 - - initial value = 0x0000 - - final XOR = 0x0001 - - reflect input = false - - reflect output = false - - check value = 0x2566 - @return CRC-15 MPT1327 parameters -*/ -inline const CRC::Parameters & CRC::CRC_15_MPT1327() -{ - static const Parameters parameters = { 0x6815, 0x0000, 0x0001, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-16 ARC (aka CRC-16 IBM, CRC-16 LHA). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 ARC has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0xBB3D - @return CRC-16 ARC parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_ARC() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 BUYPASS (aka CRC-16 VERIFONE, CRC-16 UMTS). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 BUYPASS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xFEE8 - @return CRC-16 BUYPASS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_BUYPASS() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 CCITT FALSE. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CCITT FALSE has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x29B1 - @return CRC-16 CCITT FALSE parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-16 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CDMA2000 has the following parameters and check value: - - polynomial = 0xC867 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x4C06 - @return CRC-16 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CDMA2000() -{ - static const Parameters parameters = { 0xC867, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 CMS. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CMS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xAEE7 - @return CRC-16 CMS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CMS() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DECT-R (aka CRC-16 R-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DECT-R has the following parameters and check value: - - polynomial = 0x0589 - - initial value = 0x0000 - - final XOR = 0x0001 - - reflect input = false - - reflect output = false - - check value = 0x007E - @return CRC-16 DECT-R parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DECTR() -{ - static const Parameters parameters = { 0x0589, 0x0000, 0x0001, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DECT-X (aka CRC-16 X-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DECT-X has the following parameters and check value: - - polynomial = 0x0589 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x007F - @return CRC-16 DECT-X parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DECTX() -{ - static const Parameters parameters = { 0x0589, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DNP. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DNP has the following parameters and check value: - - polynomial = 0x3D65 - - initial value = 0x0000 - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0xEA82 - @return CRC-16 DNP parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DNP() -{ - static const Parameters parameters = { 0x3D65, 0x0000, 0xFFFF, true, true }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-16 GENIBUS (aka CRC-16 EPC, CRC-16 I-CODE, CRC-16 DARC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 GENIBUS has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = false - - reflect output = false - - check value = 0xD64E - @return CRC-16 GENIBUS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_GENIBUS() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 KERMIT (aka CRC-16 CCITT, CRC-16 CCITT-TRUE). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 KERMIT has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0x2189 - @return CRC-16 KERMIT parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_KERMIT() -{ - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, true, true }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-16 MAXIM. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 MAXIM has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0x44C2 - @return CRC-16 MAXIM parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_MAXIM() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0xFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 MODBUS. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 MODBUS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0x4B37 - @return CRC-16 MODBUS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_MODBUS() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 T10-DIF. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 T10-DIF has the following parameters and check value: - - polynomial = 0x8BB7 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xD0DB - @return CRC-16 T10-DIF parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_T10DIF() -{ - static const Parameters parameters = { 0x8BB7, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 USB. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 USB has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0xB4C8 - @return CRC-16 USB parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_USB() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0xFFFF, true, true }; - return parameters; -} - -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-16 X-25 (aka CRC-16 IBM-SDLC, CRC-16 ISO-HDLC, CRC-16 B). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 X-25 has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0x906E - @return CRC-16 X-25 parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_X25() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 XMODEM (aka CRC-16 ZMODEM, CRC-16 ACORN, CRC-16 LTE). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 XMODEM has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x31C3 - @return CRC-16 XMODEM parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_XMODEM() -{ - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-17 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-17 CAN has the following parameters and check value: - - polynomial = 0x1685B - - initial value = 0x00000 - - final XOR = 0x00000 - - reflect input = false - - reflect output = false - - check value = 0x04F03 - @return CRC-17 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_17_CAN() -{ - static const Parameters parameters = { 0x1685B, 0x00000, 0x00000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-21 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-21 CAN has the following parameters and check value: - - polynomial = 0x102899 - - initial value = 0x000000 - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x0ED841 - @return CRC-21 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_21_CAN() -{ - static const Parameters parameters = { 0x102899, 0x000000, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 OPENPGP. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 OPENPGP has the following parameters and check value: - - polynomial = 0x864CFB - - initial value = 0xB704CE - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x21CF02 - @return CRC-24 OPENPGP parameters -*/ -inline const CRC::Parameters & CRC::CRC_24() -{ - static const Parameters parameters = { 0x864CFB, 0xB704CE, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 FlexRay-A. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 FlexRay-A has the following parameters and check value: - - polynomial = 0x5D6DCB - - initial value = 0xFEDCBA - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x7979BD - @return CRC-24 FlexRay-A parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() -{ - static const Parameters parameters = { 0x5D6DCB, 0xFEDCBA, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 FlexRay-B. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 FlexRay-B has the following parameters and check value: - - polynomial = 0x5D6DCB - - initial value = 0xABCDEF - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x1F23B8 - @return CRC-24 FlexRay-B parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() -{ - static const Parameters parameters = { 0x5D6DCB, 0xABCDEF, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 LTE-A/NR-A. - @note The parameters are static and are delayed-constructed to reduce memory - footprint. - @note CRC-24 LTE-A has the following parameters and check value: - - polynomial = 0x864CFB - - initial value = 0x000000 - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0xCDE703 - @return CRC-24 LTE-A parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_LTEA() -{ - static const Parameters parameters = { 0x864CFB, 0x000000, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 LTE-B/NR-B. - @note The parameters are static and are delayed-constructed to reduce memory - footprint. - @note CRC-24 LTE-B has the following parameters and check value: - - polynomial = 0x800063 - - initial value = 0x000000 - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x23EF52 - @return CRC-24 LTE-B parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_LTEB() -{ - static const Parameters parameters = { 0x800063, 0x000000, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 NR-C. - @note The parameters are static and are delayed-constructed to reduce memory - footprint. - @note CRC-24 NR-C has the following parameters and check value: - - polynomial = 0xB2B117 - - initial value = 0x000000 - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0xF48279 - @return CRC-24 NR-C parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_NRC() -{ - static const Parameters parameters = { 0xB2B117, 0x000000, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-30 CDMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-30 CDMA has the following parameters and check value: - - polynomial = 0x2030B9C7 - - initial value = 0x3FFFFFFF - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x3B3CB540 - @return CRC-30 CDMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_30() -{ - static const Parameters parameters = { 0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-32 (aka CRC-32 ADCCP, CRC-32 PKZip). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = true - - reflect output = true - - check value = 0xCBF43926 - @return CRC-32 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-32 BZIP2 (aka CRC-32 AAL5, CRC-32 DECT-B, CRC-32 B-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 BZIP2 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0xFC891918 - @return CRC-32 BZIP2 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_BZIP2() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-32 C (aka CRC-32 ISCSI, CRC-32 Castagnoli, CRC-32 Interlaken). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 C has the following parameters and check value: - - polynomial = 0x1EDC6F41 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = true - - reflect output = true - - check value = 0xE3069283 - @return CRC-32 C parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_C() -{ - static const Parameters parameters = { 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; -} -#endif - -/** - @brief Returns a set of parameters for CRC-32 MPEG-2. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 MPEG-2 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x0376E6E7 - @return CRC-32 MPEG-2 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_MPEG2() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-32 POSIX. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 POSIX has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0x00000000 - - final XOR = 0xFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0x765E7680 - @return CRC-32 POSIX parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_POSIX() -{ - static const Parameters parameters = { 0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-32 Q. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 Q has the following parameters and check value: - - polynomial = 0x814141AB - - initial value = 0x00000000 - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x3010BF7F - @return CRC-32 Q parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_Q() -{ - static const Parameters parameters = { 0x814141AB, 0x00000000, 0x00000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-40 GSM. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-40 GSM has the following parameters and check value: - - polynomial = 0x0004820009 - - initial value = 0x0000000000 - - final XOR = 0xFFFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0xD4164FC646 - @return CRC-40 GSM parameters -*/ -inline const CRC::Parameters & CRC::CRC_40_GSM() -{ - static const Parameters parameters = { 0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-64 ECMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-64 ECMA has the following parameters and check value: - - polynomial = 0x42F0E1EBA9EA3693 - - initial value = 0x0000000000000000 - - final XOR = 0x0000000000000000 - - reflect input = false - - reflect output = false - - check value = 0x6C40DF5F0B497347 - @return CRC-64 ECMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_64() -{ - static const Parameters parameters = { 0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -#ifdef CRCPP_USE_NAMESPACE -} -#endif - -#endif // CRCPP_CRC_H_ diff --git a/src/无人机端代码/Modules/communication/shard/include/Struct.hpp b/src/无人机端代码/Modules/communication/shard/include/Struct.hpp deleted file mode 100644 index 806d761d..00000000 --- a/src/无人机端代码/Modules/communication/shard/include/Struct.hpp +++ /dev/null @@ -1,900 +0,0 @@ -#ifndef STRUCT_HPP -#define STRUCT_HPP - -#include -#include // NULL -#include -#include -#include -#include - -#include -#include -#include - -//uav control -#define OPENUAVBASIC "gnome-terminal -- roslaunch prometheus_uav_control uav_control_main_outdoor.launch" -// #define CLOSEUAVBASIC "gnome-terminal -- rosnode kill /joy_node | gnome-terminal -- rosnode kill /uav_control_main_1" -//rhea control -#define OPENUGVBASIC "" -#define CLOSEUGVBASIC "" -//集群 -#define OPENSWARMCONTROL "" -#define CLOSESWARMCONTROL "" -//自主降落 -#define OPENAUTONOMOUSLANDING "" -#define CLOSEAUTONOMOUSLANDING "" -//目标识别与追踪 -#define OPENOBJECTTRACKING "" -#define CLOSEOBJECTTRACKING "" -//杀掉除了通信节点和主节点的其他节点 -//分为两种情况 -//1:杀掉子模块，这种情况不会杀掉uav control节点和通信节点以及master节点。 -//2:杀掉uav control节点，这种情况下只会保留通信节点以及master节点。 -#define CLOSEUAVBASIC "gnome-terminal -- rosnode kill `rosnode list | grep -v /ground_station_bridge | grep -v /rosout`" -#define CLOSEOTHERMODE "gnome-terminal -- rosnode kill `rosnode list | grep -v /ground_station_bridge | grep -v /rosout | grep -v /uav_control_main_1 | grep -v /joy_node`" - -//重启 -#define REBOOTNXCMD "shutdown -r now" -//关机 -#define EXITNXCMD "shutdown -h now" - - -enum MsgId -{ - UAVSTATE = 1, - TEXTINFO = 3, - GIMBALSTATE = 4, - VISIONDIFF = 5, - HEARTBEAT = 6, - RHEASTATE = 7, - MULTIDETECTIONINFO = 8, - UAVCONTROLSTATE = 9, - - SWARMCOMMAND = 101, - GIMBALCONTROL = 102, - GIMBALSERVICE = 103, - WINDOWPOSITION = 104, - RHEACONTROL = 105, - GIMBALPARAMSET = 106, - IMAGEDATA = 107, - UAVCOMMAND = 108, - UAVSETUP = 109, - - CONNECTSTATE = 201, - MODESELECTION = 202, - - //rviz数据 - UGVLASERSCAN = 230, - UGVPOINTCLOUND2 = 231, - - UGVTFMESSAGE = 232, - UGVTFSTATIC = 233, - - UGVMARKERARRAY = 234, - UGVMARKERARRAYLANDMARK = 235, - UGVMARKERARRAYTRAJECTORY = 236 -}; - -//参考文件： UAVState.msg -//订阅话题： /uav*/prometheus/state -struct Quaternion -{ - double x; - double y; - double z; - double w; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &x; - ar &y; - ar &z; - ar &w; - } -}; -//MSG 1 -struct UAVState -{ - //无人机编号 - uint8_t uav_id; - - // //无人机状态 - // uint8_t state; - // //enum agent状态枚举 - // enum State - // { - // unknown = 0, - // ready = 1, - // dead = 2, - // lost = 3 - // }; - - //无人机定位来源 - uint8_t location_source; - - //enum agent定位来源枚举 - enum LocationSource - { - MOCAP = 0, - T265 = 1, - GAZEBO = 2, - FAKE_ODOM = 3, - GPS = 4, - RTK = 5, - UWB = 6 - }; - - // PX4飞控当前飞行模式 int8 - std::string mode; - // 机载电脑是否连接上飞控，true已连接，false则不是 - bool connected; - // 是否解锁，true为已解锁，false则不是 - bool armed; - // odom失效 - bool odom_valid; - // GPS状态,变量对应状态可参考mavros_msgs/GPSRAW中的fix_type - uint8_t gps_status; - // GPS状态枚举 - enum GPSStatus - { - GPS_FIX_TYPE_NO_GPS = 0, - GPS_FIX_TYPE_NO_FIX = 1, - GPS_FIX_TYPE_2D_FIX = 2, - GPS_FIX_TYPE_3D_FIX = 3, - GPS_FIX_TYPE_DGPS = 4, - GPS_FIX_TYPE_RTK_FLOATR = 5, - GPS_FIX_TYPE_RTK_FIXEDR = 6, - GPS_FIX_TYPE_STATIC = 7, - GPS_FIX_TYPE_PPP = 8 - }; - - //无人机经度、纬度、高度 - float latitude; - float longitude; - float altitude; - - // 无人机状态量：位置、速度、姿态 - float position[3]; // [m] - float velocity[3]; // [m/s] - float attitude[3]; // [rad] - Quaternion attitude_q; // 四元数 - float attitude_rate[3]; // [rad/s] - float battery_state; // 电池状态[v] - float battery_percetage; // [0-1] - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &uav_id; - // ar &state; - ar &location_source; - ar &connected; - ar &mode; - ar &armed; - ar &odom_valid; - ar &gps_status; - ar &latitude; - ar &longitude; - ar &altitude; - ar &position; - ar &velocity; - ar &attitude; - ar &attitude_q; - ar &attitude_rate; - ar &battery_state; - ar &battery_percetage; - } -}; - -struct Heartbeat -{ - uint32_t count; - std::string message; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &count; - ar &message; - } -}; - -struct RheaState -{ - uint8_t rhea_id; - double linear; - double angular; - double yaw; - float latitude; - float longitude; - float altitude; - float position[3]; - float battery_voltage; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &rhea_id; - ar &linear; - ar &angular; - ar &yaw; - ar &latitude; - ar &longitude; - ar &altitude; - ar &position; - ar &battery_voltage; - } -}; - -struct ModeSelection -{ - uint8_t mode; - enum Mode - { - UAVBASIC = 1, - UGVBASIC = 2, - SWARMCONTROL = 3, - //GIMBAL? - AUTONOMOUSLANDING = 4, - OBJECTTRACKING = 5, - CUSTOMMODE = 6, - REBOOTNX = 7, - EXITNX = 8 - }; -// bool is_simulation; - std::vector selectId; - // uint8_t swarm_num; - - uint8_t use_mode; - enum UseMode - { - CREATE = 0, - DELETE = 1 - }; - - bool is_simulation; - - int swarm_num; - - std::string cmd; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &selectId; - ar &mode; - ar &use_mode; - ar &is_simulation; - ar &swarm_num; - ar &cmd; - } -}; - -struct Point -{ - double x; - double y; - double z; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &x; - ar &y; - ar &z; - } -}; - -// SwarmCommand.msg -// /prometheus/swarm_command -struct SwarmCommand -{ - // 消息来源 - std::string source; - - //编队套件数量 - uint8_t swarm_num; - - //定位来源 - uint8_t swarm_location_source; - enum SwarmLocationSource - { - mocap = 0, - gps = 4, - rtk = 5, - uwb = 6 - }; - - // 命令 - uint8_t Swarm_CMD; - // enum CMD 控制模式枚举 - // enum CMD 控制模式枚举 - enum SwarmCMD - { - Ready = 0, - Init = 1, - Start = 2, - Hold = 3, - Stop = 4, - Formation = 5, - Follow = 11, - Search = 12, - Attack = 13 - }; - - // 编队控制参考量 - float leader_pos[3]; - float leader_vel[2]; - float swarm_size; - uint8_t swarm_shape; - enum SwarmShape - { - One_column = 0, - Triangle = 1, - Square = 2, - Circular = 3 - }; - - // 搜索控制参考量 - float target_area_x_min; // [m] - float target_area_y_min; // [m] - float target_area_x_max; // [m] - float target_area_y_max; // [m] - - // 攻击控制参考量 - float attack_target_pos[3]; // [m] - - std::vector formation_poses; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &source; - ar &swarm_num; - ar &swarm_location_source; - ar &Swarm_CMD; - ar &leader_pos; - ar &leader_vel; - ar &swarm_size; - ar &swarm_shape; - ar &target_area_x_min; - ar &target_area_y_min; - ar &target_area_x_max; - ar &target_area_y_max; - ar &attack_target_pos; - ar &formation_poses; - } -}; - -// StationFeedback.msg -// /uav*/prometheus/station_feedback -struct TextInfo -{ - enum MessageTypeGrade - { - //INFO:正常运行状况下反馈给地面站的信息,例如程序正常启动,状态切换的提示信息等. - INFO = 0, - //WARN:无人机或软件程序出现意外情况,依然能正常启动或继续执行任务,小概率会出现危险状况,例如无人机RTK无法维持退出到GPS,视觉跟踪目标突然丢失重新搜寻目标等. - WARN = 1, - //ERROR:无人机或软件程序出现重大意外情况,无法正常启动或继续执行任务,极有可能会出现危险状况,需要中断任务以及人为接管控制无人机,例如通信中断,无人机定位发散,ROS节点无法正常启动等. - ERROR = 2, - //FATAL:任务执行过程中,软件崩溃或无人机飞控崩溃导致无人机完全失控,需要迅速人为接管控制无人机降落减少炸机损失. - FATAL = 3 - }; - - int sec; - uint8_t MessageType; - std::string Message; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &sec; - ar &MessageType; - ar &Message; - } -}; - -//参考文件： GimbalState.msg -//订阅话题： ~/gimbal/state -struct GimbalState -{ - uint8_t Id; - //# 0: 发一句回一句 `# 1: 发一句，一直回复, 摄像头倍数返回将失效`` - uint8_t feedbackMode; - //mode 0:手动控制 1:home 2:tracking 3:yaw follow 4:hold吊舱跟随无人机 5:search - uint8_t mode; - //是否视频录制 - bool isRecording; - //# 是否开启自动缩放(VisionDiff需要指定面积才能生效) 0:保持 1:放大 2:缩小 3:自动 - uint8_t zoomState; - // 当前所处倍数 - float zoomVal; - //roll,pitch,yaw - float imuAngle[3]; - //Current gimbal joint angles(roll,pitch,yaw), published at 30 Hz. - float rotorAngle[3]; - //rpy_vel 角速度 - float imuAngleVel[3]; - //rpy_tgt 目标角度 - float rotorAngleTarget[3]; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &Id; - ar &feedbackMode; - ar &mode; - ar &isRecording; - ar &zoomState; - ar &zoomVal; - ar &imuAngle; - ar &rotorAngle; - ar &imuAngleVel; - ar &rotorAngleTarget; - } -}; -//参考文件： VisionDiff.msg -//订阅话题： ~/gimbal/track -struct VisionDiff -{ - uint8_t id; - uint8_t detect; - - uint16_t objectX; - uint16_t objectY; - uint16_t objectWidth; - uint16_t objectHeight; - - uint16_t frameWidth; - uint16_t frameHeight; - - // Gimbal 跟踪pid - float kp; - float ki; - float kd; - - int8_t ctlMode; // 0: yaw+pitch, 1: roll+pitch 3:混合(未实现) - enum CtlMode - { - yawPitch = 0, - rollPitch = 1, - mix = 3 - }; - - float currSize; //框选近大远小 - float maxSize; - float minSize; //框选大小 - - float trackIgnoreError; - bool autoZoom; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &id; - ar &objectX; - ar &objectY; - ar &objectWidth; - ar &objectHeight; - ar &frameWidth; - ar &frameHeight; - ar &kp; - ar &ki; - ar &kd; - ar &ctlMode; - ar &currSize; - ar &maxSize; - ar &minSize; - ar &trackIgnoreError; - ar &autoZoom; - } -}; - -//参考文件： GimbalControl.msg -//订阅话题： ~/gimbal/control -struct GimbalControl -{ - uint8_t Id; - //control mode 0:nothong 1:angle 2:speed 3:home postion - uint8_t rpyMode; - enum RPYMode - { - manual = 1, - home = 2, - hold = 3, - fellow = 4 - }; - - uint8_t roll; - uint8_t yaw; - uint8_t pitch; - - enum ControlMode - { - noCtl = 0, - velocityCtl = 1, - angleCtl = 2 - }; - - float rValue; // deg 单位 - float yValue; // deg - float pValue; // deg - - // focus - uint8_t focusMode; // 默认值 - enum FocusMode - { - focusStop = 1, - focusOut = 2, - focusIn = 3 - }; - - // zoom - uint8_t zoomMode; // 默认值 - enum ZoomMode - { - zoomStop = 1, - zoomOut = 2, - zoomIn = 3 - }; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &Id; - ar &rpyMode; - ar &roll; - ar &yaw; - ar &pitch; - ar &rValue; - ar &yValue; - ar &pValue; - ar &focusMode; - ar &zoomMode; - } -}; - -struct GimbalService -{ - uint8_t service; - enum Service - { - search = 1, - record_video = 2, - track_mode = 3 - }; - - bool data; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &service; - ar &data; - } -}; - -struct GimbalParamSet -{ - std::string param_id; - double real; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar ¶m_id; - ar ℜ - } -}; - -struct WindowPosition -{ - //模式：... - uint8_t mode; - enum Mode - { - IDLE = 0, - RECTANGLE = 1, - POINT = 2, - TRACK_ID = 3, - FRAME_ID_AND_POINT = 4 - }; - - //波门位置X,//波门位置Y(kcf,点击画面的功能的时候使用),左上角为（0,0） - int16_t origin_x; - int16_t origin_y; - int16_t width; - int16_t height; - - //波门位置X,//波门位置Y - //int16 window_position_x = origin_x + width/2 - //int16 window_position_y = origin_y + height/2 - int16_t window_position_x; - int16_t window_position_y; - - //算法检测结果的ID - int32_t track_id; - - // //被点击或框选帧（画面暂停的ID） - // int32_t frame_id; - - std::string udp_msg; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &mode; - ar &origin_x; - ar &origin_y; - ar &width; - ar &height; - ar &window_position_x; - ar &window_position_y; - ar &track_id; - // ar &frame_id; - ar &udp_msg; - } -}; - -//ROS话题: "/deepsort_ros/object_detection_result" -struct DetectionInfo -{ - //目标框的位置（主要斜对角两个点） - float left; - float top; - float bot; - float right; - - //TRACK TARGET - int32_t trackIds; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &left; - ar ⊤ - ar ⊥ - ar &right; - ar &trackIds; - } -}; -struct MultiDetectionInfo -{ - //模式：0：空闲 2.simaRPN 3.deepsort/sort - uint8_t mode; - - //检测到的目标数量 - int32_t num_objs; - - //每个目标的检测结果 - //DetectionInfo[] detection_infos; - std::vector detection_infos; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &mode; - ar &num_objs; - ar &detection_infos; - } -}; - -struct RheaGPS -{ - double latitude; - double longitude; - double altitude; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &latitude; - ar &longitude; - ar &altitude; - } -}; - -struct RheaControl -{ - - uint8_t Mode; - - //控制模式类型枚举 - enum Mode - { - Stop = 0, - Forward = 1, - Left = 2, - Right = 3, - Back = 4, - CMD = 5, - Waypoint = 6 - }; - - double linear; - double angular; - - std::vector waypoint; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &Mode; - ar &linear; - ar &angular; - ar &waypoint; - } -}; - -struct ImageData -{ - std::string name; - std::string data; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &name; - ar &data; - } -}; - -struct UAVCommand -{ - //控制命令的模式 - uint8_t Agent_CMD; - //Agent_CMD 枚举 - enum AgentCMD - { - Init_Pos_Hover = 1, - Current_Pos_Hover = 2, - Land = 3, - Move = 4, - User_Mode1 = 5 - }; - - //移动命令下的子模式 - uint8_t Move_mode; - // 移动命令下的子模式枚举 - enum MoveMode - { - XYZ_POS = 0, // 惯性系定点控制 - XY_VEL_Z_POS = 1, // 惯性系定高速度控制 - XYZ_VEL = 2, // 惯性系速度控制 - XYZ_POS_BODY = 3, // 机体系位置控制 - XYZ_VEL_BODY = 4, // 机体系速度控制 - XY_VEL_Z_POS_BODY = 5, // 机体系定高速度控制 - TRAJECTORY = 6, // 轨迹追踪控制 - XYZ_ATT = 7, // 姿态控制（来自外部控制器） - LAT_LON_ALT = 8 // 绝对坐标系下的经纬度 - }; - // 控制参考量 - float position_ref[3]; // [m] - float velocity_ref[3]; // [m/s] - float acceleration_ref[3]; // [m/s^2] - float yaw_ref; // [rad] - bool Yaw_Rate_Mode; // True 代表控制偏航角速率 - float yaw_rate_ref; // [rad/s] - float att_ref[4]; // [rad] + [0-1] - double latitude; // 无人机经度、纬度、高度 - double longitude; // 无人机经度、纬度、高度 - double altitude; // 无人机经度、纬度、高度 - - // 控制命令的编号 防止接收到错误命令， 编号应该逐次递加 - uint Command_ID; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &Agent_CMD; - ar &Move_mode; - ar &position_ref; - ar &velocity_ref; - ar &acceleration_ref; - ar &yaw_ref; - ar &Yaw_Rate_Mode; - ar &yaw_rate_ref; - ar &att_ref; - ar &Command_ID; - ar &latitude; - ar &longitude; - ar &altitude; - } -}; - -struct UAVSetup -{ - //Setup类型(模拟遥控器) - uint8_t cmd; - enum CMD - { - ARMING = 0, - SET_PX4_MODE = 1, - REBOOT_PX4 = 2, - SET_CONTROL_MODE = 3, - }; - - bool arming; - // http://wiki.ros.org/mavros/CustomModes ,可参考该网址设置模式名,常用模式名:OFFBOARD,AUTO.LAND,AUTO.RTL,POSCTL - std::string px4_mode; - std::string control_state; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &cmd; - ar &arming; - ar &px4_mode; - ar &control_state; - } -}; - -struct UAVControlState -{ - // 无人机编号 - uint8_t uav_id; - - // 无人机控制状态 - uint8_t control_state; - // 状态枚举 - enum ControlState - { - INIT = 0, - MANUAL_CONTROL = 1, - HOVER_CONTROL = 2, - COMMAND_CONTROL = 3, - LAND_CONTROL = 4 - }; - - // 无人机控制器标志量 - uint8_t pos_controller; - // 状态枚举 - enum ControllerFlag - { - PX4_ORIGIN = 0, - PID = 1, - UDE = 2, - NE = 3 - }; - - // 无人机安全保护触发标志量 - bool failsafe; - - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar &uav_id; - ar &control_state; - ar &pos_controller; - ar &failsafe; - } -}; - -struct ConnectState -{ - uint8_t num; - bool state; - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int /* file_version */) - { - ar # - ar &state; - } -}; - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/shard/include/communication.hpp b/src/无人机端代码/Modules/communication/shard/include/communication.hpp deleted file mode 100644 index 150355fa..00000000 --- a/src/无人机端代码/Modules/communication/shard/include/communication.hpp +++ /dev/null @@ -1,98 +0,0 @@ -#ifndef COMUNICATION_HPP -#define COMUNICATION_HPP - -#include -#include -#include -#include -#include -#include - -#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 - int encodeMsg(int8_t send_mode, T msg); - - //解码 - int decodeMsg(char *buff); - - //根据传入的struct返回对应的MSG_ID - template - uint8_t getMsgId(T msg); - - template - 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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/shard/libs/libcommunication.so b/src/无人机端代码/Modules/communication/shard/libs/libcommunication.so deleted file mode 100644 index 35766967..00000000 Binary files a/src/无人机端代码/Modules/communication/shard/libs/libcommunication.so and /dev/null differ diff --git a/src/无人机端代码/Modules/communication/src/autonomous_landing_topic.cpp b/src/无人机端代码/Modules/communication/src/autonomous_landing_topic.cpp deleted file mode 100644 index ff6b308d..00000000 --- a/src/无人机端代码/Modules/communication/src/autonomous_landing_topic.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include "autonomous_landing_topic.hpp" - -AutonomousLanding::AutonomousLanding(ros::NodeHandle &nh,Communication *communication) -{ - nh.param("ROBOT_ID", robot_id, 0); - nh.param("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("/gimbal/search"); - //【服务】是否开启视频录制 - this->gimbal_record_video_client_ = nh.serviceClient("/gimbal/record_video"); - //【服务】是否自动反馈(真实IMU速度) - this->gimbal_track_mode_client_ = nh.serviceClient("/gimbal/track_mode"); - //【服务】自主降落参数配置 - this->param_set_client_ = nh.serviceClient("/autonomous_landing/ParamSet"); - //【发布】无人车数据 - this->ugv_state_pub_ = nh.advertise("/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_); -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/src/communication_bridge.cpp b/src/无人机端代码/Modules/communication/src/communication_bridge.cpp deleted file mode 100644 index 6ebed95e..00000000 --- a/src/无人机端代码/Modules/communication/src/communication_bridge.cpp +++ /dev/null @@ -1,843 +0,0 @@ -#include "communication_bridge.hpp" -#include -#include -std::mutex g_m; -std::mutex g_uav_basic; -// boost::shared_mutex g_m; - -#define DEMO1 "gnome-terminal -- roslaunch prometheus_demo takeoff_land.launch" - -CommunicationBridge::CommunicationBridge(ros::NodeHandle &nh) : Communication() -{ - //是否仿真 1 为是 0为否 - nh.param("is_simulation", this->is_simulation_, 1); - //集群数量 非集群模式值为0 - nh.param("swarm_num", this->swarm_num_, 0); - //载体类型 - nh.param("user_type", this->user_type_, 1); - //集群模式下数据更新超时多久进行反馈 - nh.param("swarm_data_update_timeout", this->swarm_data_update_timeout_, 5); - - nh.param("udp_port", UDP_PORT, 8889); - nh.param("tcp_port", TCP_PORT, 55555); - nh.param("tcp_heartbeat_port", TCP_HEARTBEAT_PORT, 55556); - // nh.param("rviz_port", RVIZ_PORT, 8890); - nh.param("ROBOT_ID", ROBOT_ID, 1); - nh.param("ground_stationt_ip", udp_ip, "127.0.0.1"); - nh.param("multicast_udp_ip", multicast_udp_ip, "224.0.0.88"); - nh.param("try_connect_num", try_connect_num, 3); - - this->nh_ = nh; - - Communication::init(ROBOT_ID,UDP_PORT,TCP_PORT,TCP_HEARTBEAT_PORT); - - bool auto_start = false; - this->nh_.param("auto_start", auto_start, false); - //自动启动话题 - if (auto_start == true) - init(); - - // thread_recCommunicationBridgeiver - boost::thread recv_thd(&CommunicationBridge::serverFun, this); - recv_thd.detach(); //后台 - ros::Duration(1).sleep(); // wait - - // thread_receiver - boost::thread recv_multicast_thd(&CommunicationBridge::multicastUdpFun, this); - recv_multicast_thd.detach(); - ros::Duration(1).sleep(); // wait - - boost::thread to_ground_station_thd(&CommunicationBridge::toGroundStationFun, this); - to_ground_station_thd.detach(); - ros::Duration(1).sleep(); // wait - - // system(OPENUAVBASIC); -} - -CommunicationBridge::~CommunicationBridge() -{ - if (this->uav_basic_ != NULL) - delete this->uav_basic_; - if (this->ugv_basic_ != NULL) - delete this->ugv_basic_; - if (this->autonomous_landing_ != NULL) - delete this->autonomous_landing_; - if (this->object_tracking_ != NULL) - delete this->object_tracking_; - if (this->swarm_control_ != NULL) - delete this->swarm_control_; -} - -void CommunicationBridge::init() -{ - // if (this->user_type_ == 1) - // { - // this->uav_basic_ = new UAVBasic(this->nh_, ROBOT_ID); - // if (this->is_simulation_ == 1) - // { - // if (this->swarm_num_ != 0) - // { - // for (int i = 1; i <= this->swarm_num_; i++) - // { - // if (i == ROBOT_ID) - // { - // this->swarm_control_simulation_[i] = this->uav_basic_; - // continue; - // } - // this->swarm_control_simulation_[i] = new UAVBasic(this->nh_, i); - // } - // this->swarm_control_ = new SwarmControl(this->nh_,this->swarm_num_); - // } - // }else - // { - // if(this->swarm_num_ != 0) - // this->swarm_control_ = new SwarmControl(this->nh_,ROBOT_ID,this->swarm_num_); - // } - // this->gimbal_basic_ = new GimbalBasic(this->nh_); - // this->object_tracking_ = new ObjectTracking(this->nh_); - // this->autonomous_landing_ = new AutonomousLanding(this->nh_); - // } - //根据载体进行初始化 - if (this->user_type_ == UserType::UAV) - { - this->uav_basic_ = new UAVBasic(this->nh_, ROBOT_ID,(Communication*)this); - } - else if (this->user_type_ == UserType::UGV) - { - this->ugv_basic_ = new UGVBasic(this->nh_,(Communication*)this); - } -} - -//TCP服务端 -void CommunicationBridge::serverFun() -{ - int valread; - if (waitConnectionFromGroundStation(TCP_PORT) < 0) - { - ROS_ERROR("[bridge_node]Socket recever creation error!"); - exit(EXIT_FAILURE); - } - - while (true) - { - //等待连接队列中抽取第一个连接，创建一个与s同类的新的套接口并返回句柄。 - if ((recv_sock = accept(server_fd, (struct sockaddr *)NULL, NULL)) < 0) - { - perror("accept"); - exit(EXIT_FAILURE); - } - - //recv函数从TCP连接的另一端接收数据 - valread = recv(recv_sock, tcp_recv_buf, BUF_LEN, 0); - usleep(200000); - - if (valread <= 0) - { - ROS_ERROR("Received message length <= 0, maybe connection has lost"); - close(recv_sock); - continue; - } - - // std::lock_guard lg(g_m); - - std::cout << "tcp valread: " << valread << std::endl; - //char *ptr = tcp_recv_buf; - //目前只有地面站发送TCP消息、所以TCP服务端接收到数据后开始心跳包的发送 - this->is_heartbeat_ready_ = true; - - pubMsg(decodeMsg(tcp_recv_buf)); - close(recv_sock); - } -} - -void CommunicationBridge::recvData(struct UAVState uav_state) -{ - - if (this->swarm_control_ != NULL) - { - //融合到所有无人机状态然后发布话题 - this->swarm_control_->updateAllUAVState(uav_state); - //发布话题 - this->swarm_control_->allUAVStatePub(this->swarm_control_->getMultiUAVState()); - } -} -void CommunicationBridge::recvData(struct UAVCommand uav_cmd) -{ - if (this->uav_basic_ == NULL) - { - return; - } - this->uav_basic_->uavCmdPub(uav_cmd); -} -void CommunicationBridge::recvData(struct SwarmCommand swarm_command) -{ - if (this->swarm_control_ == NULL) - { - return; - } - if (swarm_command.swarm_num != this->swarm_num_) - { - struct TextInfo text_info; - text_info.MessageType = text_info.WARN; - text_info.Message = "ground station swarm num ！= communication module swarm num"; - text_info.sec = ros::Time::now().sec; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - } - //发布话题 - this->swarm_control_->swarmCommandPub(swarm_command); -} -void CommunicationBridge::recvData(struct ConnectState connect_state) -{ - if (this->is_simulation_ == 0) - return; - if (!connect_state.state || connect_state.num < this->swarm_num_) - //this->swarm_control_->closeUAVState(connect_state.num); - //触发降落信号 - this->swarm_control_->communicationStatePub(connect_state.state, connect_state.num); -} -void CommunicationBridge::recvData(struct GimbalControl gimbal_control) -{ - if (this->gimbal_basic_ == NULL) - { - return; - } - this->gimbal_basic_->gimbalControlPub(gimbal_control); -} -void CommunicationBridge::recvData(struct GimbalService gimbal_service) -{ - if (this->autonomous_landing_ == NULL) - { - return; - } - if (gimbal_service.service == gimbal_service.search) - this->autonomous_landing_->gimbalSearchServer(gimbal_service.data); - else if (gimbal_service.service == gimbal_service.record_video) - this->autonomous_landing_->gimbalRecordVideoServer(gimbal_service.data); - else if (gimbal_service.service == gimbal_service.track_mode) - this->autonomous_landing_->gimbalTrackModeServer(gimbal_service.data); -} -void CommunicationBridge::recvData(struct GimbalParamSet param_set) -{ - if (this->autonomous_landing_ == NULL) - { - return; - } - this->autonomous_landing_->gimbalParamSetServer(param_set); -} -void CommunicationBridge::recvData(struct WindowPosition window_position) -{ - if (this->gimbal_basic_ == NULL) - { - return; - } - //如果udp_msg数据不为空 则向udo端口发送数据。否则发布ros话题 - if (!window_position.udp_msg.empty()) - { - std::cout << "udp_msg size :" << window_position.udp_msg.size() << std::endl; - sendMsgByUdp(window_position.udp_msg.size(), window_position.udp_msg.c_str(), "127.0.0.1", SERV_PORT); - } - else - { - this->gimbal_basic_->gimbalWindowPositionPub(window_position); - } -} -void CommunicationBridge::recvData(struct RheaControl rhea_control) -{ - if (this->ugv_basic_ == NULL) - { - return; - } - this->ugv_basic_->rheaControlPub(rhea_control); -} -void CommunicationBridge::recvData(struct RheaState rhea_state) -{ - if (this->autonomous_landing_ == NULL) - { - return; - } - this->autonomous_landing_->rheaStatePub(rhea_state); -} -void CommunicationBridge::recvData(struct ImageData image_data) -{ - createImage(image_data); -} -void CommunicationBridge::recvData(struct UAVSetup uav_setup) -{ - if (this->uav_basic_ == NULL) - { - return; - } - this->uav_basic_->uavSetupPub(uav_setup); -} -void CommunicationBridge::recvData(struct ModeSelection mode_selection) -{ - modeSwitch(mode_selection); -} - -//根据协议中MSG_ID的值，将数据段数据转化为正确的结构体 -void CommunicationBridge::pubMsg(int msg_id) -{ - switch (msg_id) - { - case MsgId::UAVSTATE: - recvData(recv_uav_state_); - break; - case MsgId::SWARMCOMMAND: - recvData(recv_swarm_command_); - break; - case MsgId::CONNECTSTATE: - //集群仿真下有效 - recvData(recv_connect_state_); - break; - case MsgId::GIMBALCONTROL: - recvData(recv_gimbal_control_); - break; - case MsgId::GIMBALSERVICE: - recvData(recv_gimbal_service_); - break; - case MsgId::GIMBALPARAMSET: - recvData(recv_param_set_); - break; - case MsgId::WINDOWPOSITION: - recvData(recv_window_position_); - break; - case MsgId::RHEACONTROL: - recvData(recv_rhea_control_); - break; - case MsgId::RHEASTATE: - recvData(recv_rhea_state_); - break; - case MsgId::IMAGEDATA: - recvData(recv_image_data_); - break; - case MsgId::UAVCOMMAND: - recvData(recv_uav_cmd_); - break; - case MsgId::UAVSETUP: - recvData(recv_uav_setup_); - break; - case MsgId::MODESELECTION: - recvData(recv_mode_selection_); - break; - default: - break; - } -} - -void CommunicationBridge::createImage(struct ImageData image_data) -{ - std::ofstream os(image_data.name); - os << image_data.data; - os.close(); - std::cout << "image_data" << std::endl; -} - -void CommunicationBridge::modeSwitch(struct ModeSelection mode_selection) -{ - //test - // if ((int)mode_selection.mode == 6) - // { - // system(DEMO1); - // return; - // } - if (mode_selection.mode == ModeSelection::Mode::REBOOTNX) - { - system(REBOOTNXCMD); - } - else if (mode_selection.mode == ModeSelection::Mode::EXITNX) - { - system(EXITNXCMD); - } - - struct TextInfo text_info; - - text_info.sec = ros::Time::now().sec; - if (mode_selection.use_mode == ModeSelection::UseMode::CREATE) - { - if (createMode(mode_selection)) - { - text_info.MessageType = TextInfo::MessageTypeGrade::INFO; - text_info.Message = "open mode success!"; - } - else - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "open mode fail!"; - } - } - else if (mode_selection.use_mode == ModeSelection::UseMode::DELETE) - { - if (deleteMode(mode_selection)) - { - text_info.MessageType = TextInfo::MessageTypeGrade::INFO; - text_info.Message = "close mode success!"; - } - else - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "close mode fail!"; - } - } - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); -} - -bool CommunicationBridge::createMode(struct ModeSelection mode_selection) -{ - struct TextInfo text_info; - text_info.MessageType = TextInfo::MessageTypeGrade::INFO; - text_info.sec = ros::Time::now().sec; - bool is = true; - if (mode_selection.mode == ModeSelection::Mode::UAVBASIC) - { - //仿真模式 允许同一通信节点创建多个飞机的话题 - if (this->is_simulation_ == 1) - { - for (int i = 0; i < mode_selection.selectId.size(); i++) - { - //判断是否已经存在 - if (!this->swarm_control_simulation_.empty()) - { - if (this->swarm_control_simulation_.find(mode_selection.selectId[i]) != this->swarm_control_simulation_.end()) - { - text_info.Message = "UAVBasic simulation id :" + to_string(mode_selection.selectId[i]) + " already exists"; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - return false; - } - } - - //创建并存入 - this->swarm_control_simulation_[mode_selection.selectId[i]] = new UAVBasic(this->nh_, mode_selection.selectId[i],(Communication*)this); - text_info.Message = "create UAVBasic simulation id :" + to_string(mode_selection.selectId[i]) + "..."; - //如果id与通信节点相同则存入uav_basic_ - if (ROBOT_ID == mode_selection.selectId[i]) - { - if (this->uav_basic_ != NULL) - { - return false; - } - - this->uav_basic_ = this->swarm_control_simulation_[mode_selection.selectId[i]]; - - //打开 - // system(OPENUAVBASIC); - } - text_info.Message = "create UAVBasic simulation id :" + to_string(mode_selection.selectId[0]) + "..."; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - } - } - //真机模式 同一通信节点只能创建一个飞机的话题 - else - { - for (int i = 0; i < mode_selection.selectId.size(); i++) - { - //如果id与通信节点相同则存入uav_basic_ - if (mode_selection.selectId[i] == ROBOT_ID) - { - if (this->uav_basic_ == NULL) - { - this->uav_basic_ = new UAVBasic(this->nh_, ROBOT_ID,(Communication*)this); - text_info.Message = "create UAVBasic :" + to_string(ROBOT_ID) + "..."; - - //启动 uav_control节点 - //先关闭防止重复打开 - // system(CLOSEUAVBASIC); - //打开 - // system(OPENUAVBASIC); - } - } - else - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "id inconformity"; - is = false; - } - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - } - } - } - else if (mode_selection.mode == ModeSelection::Mode::UGVBASIC) - { - if (this->ugv_basic_ == NULL) - { - this->ugv_basic_ = new UGVBasic(this->nh_,(Communication*)this); - text_info.Message = "UGVBasic"; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - system(CLOSEUGVBASIC); - system(OPENUGVBASIC); - } - } - //集群模式 - else if (mode_selection.mode == ModeSelection::Mode::SWARMCONTROL) - { - if (this->swarm_num_ != mode_selection.selectId.size()) - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "mode switch fail，because swarm num inconsistent."; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - return false; - } - if (this->is_simulation_ != mode_selection.is_simulation) - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "mode switch fail，because mode inconsistent."; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - return false; - } - //仿真模式 - if (this->is_simulation_ == 1) - { - for (int i = 0; i < mode_selection.selectId.size(); i++) - { - if (this->swarm_control_simulation_.count(mode_selection.selectId[i]) == 0) - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "mode switch fail，id " + to_string(mode_selection.selectId[i]) + " non-existent"; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - return false; - } - } - if (this->swarm_control_ == NULL) - { - this->swarm_control_ = new SwarmControl(this->nh_, this->swarm_num_,(Communication*)this); - //this->swarm_control_ = std::make_shared(this->nh_, this->swarm_num); - text_info.Message = "simulation SwarmControl: swarm_num:" + std::to_string(this->swarm_num_); - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - system(OPENSWARMCONTROL); - } - } - else //真机 - { - for (int i = 0; i < mode_selection.selectId.size(); i++) - { - if (mode_selection.selectId[i] == ROBOT_ID) - { - this->swarm_control_ = new SwarmControl(this->nh_, ROBOT_ID, this->swarm_num_,(Communication*)this); - text_info.Message = "SwarmControl: swarm_num:" + std::to_string(this->swarm_num_); - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - break; - } - if (i == mode_selection.selectId.size() - 1) - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "mode switch fail，id " + to_string(ROBOT_ID) + " non-existent"; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - return false; - } - } - } - - //启动子模块指令 - //system() - } - - else if (mode_selection.mode == ModeSelection::Mode::AUTONOMOUSLANDING) - { - if (this->ugv_basic_ != NULL) - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "mode switch fail，because user type ugv."; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - return false; - } - if (this->uav_basic_ != NULL) - { - if (this->gimbal_basic_ == NULL) - { - this->gimbal_basic_ = new GimbalBasic(this->nh_,(Communication*)this); - } - //自主降落 - if (this->autonomous_landing_ == NULL) - { - this->autonomous_landing_ = new AutonomousLanding(this->nh_,(Communication*)this); - } - text_info.Message = "AutonomousLanding"; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - system(OPENAUTONOMOUSLANDING); - } - } - //目标识别与跟踪模式 - else if (mode_selection.mode == ModeSelection::Mode::OBJECTTRACKING) - { - if (this->uav_basic_ != NULL) - { - if (this->gimbal_basic_ == NULL) - { - this->gimbal_basic_ = new GimbalBasic(this->nh_,(Communication*)this); - } - if (this->object_tracking_ == NULL) - { - this->object_tracking_ = new ObjectTracking(this->nh_,(Communication*)this); - } - text_info.Message = "ObjectTracking"; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - system(OPENOBJECTTRACKING); - } - } - else if (mode_selection.mode == ModeSelection::Mode::CUSTOMMODE) - { - system(mode_selection.cmd.c_str()); - } - this->current_mode_ = mode_selection.mode; - return is; -} - -bool CommunicationBridge::deleteMode(struct ModeSelection mode_selection) -{ - struct TextInfo text_info; - text_info.MessageType = TextInfo::MessageTypeGrade::INFO; - text_info.sec = ros::Time::now().sec; - - if (mode_selection.mode == ModeSelection::Mode::UAVBASIC) - { - if (this->is_simulation_ == 1) - { - for (int i = 0; i < mode_selection.selectId.size(); i++) - { - std::lock_guard lg_uav_basic(g_uav_basic); - if (this->swarm_control_simulation_.find(mode_selection.selectId[i]) != this->swarm_control_simulation_.end()) - { - delete this->swarm_control_simulation_[mode_selection.selectId[i]]; - this->swarm_control_simulation_.erase(this->swarm_control_simulation_.find(mode_selection.selectId[i])); - - if (ROBOT_ID == mode_selection.selectId[i]) - { - // delete this->uav_basic_; - this->uav_basic_ = NULL; - // system(CLOSEUAVBASIC); - } - } - text_info.Message = "delete UAVBasic simulation id :" + to_string(mode_selection.selectId[i]) + "..."; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - } - } - else - { - for (int i = 0; i < mode_selection.selectId.size(); i++) - { - if (ROBOT_ID == mode_selection.selectId[i]) - { - if (this->uav_basic_ != NULL) - { - delete this->uav_basic_; - this->uav_basic_ = NULL; - system(CLOSEUAVBASIC); - text_info.Message = "delete UAVBasic id :" + to_string(mode_selection.selectId[i]) + "..."; - } - } - else - { - text_info.MessageType = TextInfo::MessageTypeGrade::WARN; - text_info.Message = "id inconformity"; - return false; - } - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - } - } - } - else if (mode_selection.mode == ModeSelection::Mode::UGVBASIC) - { - if (this->ugv_basic_ != NULL) - { - delete this->ugv_basic_; - this->ugv_basic_ = NULL; - system(CLOSEUGVBASIC); - } - } - else if (mode_selection.mode == ModeSelection::Mode::SWARMCONTROL) - { - // std::lock_guard lg(g_m); - if (this->swarm_control_ != NULL) - { - //开启互斥锁 - //boost::unique_lock lockImageStatus(g_m); - std::lock_guard lg(g_m); - delete this->swarm_control_; - this->swarm_control_ = NULL; - system(CLOSEOTHERMODE); - } - } - else if (mode_selection.mode == ModeSelection::Mode::AUTONOMOUSLANDING) - { - if (this->autonomous_landing_ != NULL) - { - delete this->autonomous_landing_; - this->autonomous_landing_ = NULL; - system(CLOSEOTHERMODE); - } - } - else if (mode_selection.mode == ModeSelection::Mode::OBJECTTRACKING) - { - if (this->object_tracking_ != NULL) - { - delete this->object_tracking_; - this->object_tracking_ = NULL; - system(CLOSEOTHERMODE); - } - } - return true; -} - -//接收组播地址的数据 -void CommunicationBridge::multicastUdpFun() -{ - if (this->swarm_num_ == 0) - { - return; - } - while (this->is_simulation_ == 1) - { - std::lock_guard lg_uav_basic(g_uav_basic); - for (auto it = this->swarm_control_simulation_.begin(); it != this->swarm_control_simulation_.end(); it++) - { - //开启互斥锁 - //boost::shared_lock lock(g_m); - std::lock_guard lg(g_m); - if (this->swarm_control_ != NULL) - { - this->swarm_control_->updateAllUAVState(it->second->getUAVState()); - this->swarm_control_->allUAVStatePub(this->swarm_control_->getMultiUAVState()); - } - } - } - int valread; - if (waitConnectionFromMulticast(UDP_PORT) < 0) - { - ROS_ERROR("[bridge_node]Socket recever creation error!"); - exit(EXIT_FAILURE); - } - - struct ip_mreq mreq; - mreq.imr_multiaddr.s_addr = inet_addr(multicast_udp_ip.c_str()); - - setsockopt(udp_socket, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); - sockaddr_in srv_Addr; //用于存储发送方信息 - socklen_t addr_len = sizeof(srv_Addr); - - while (true) - { - std::lock_guard lg(g_m); - if (this->swarm_control_ == NULL) - { - continue; - } - - valread = recvfrom(udp_socket, udp_recv_buf, BUF_LEN, 0, (struct sockaddr *)&srv_Addr, &addr_len); - - if (valread <= 0) - { - ROS_ERROR("Received message length <= 0, maybe connection has lost"); - continue; - } - - //std::lock_guard lg(g_m); - - std::cout << "udp valread: " << valread << std::endl; - pubMsg(decodeMsg(udp_recv_buf)); - } -} - -//给地面站发送心跳包, 超时检测 -void CommunicationBridge::toGroundStationFun() -{ - struct Heartbeat heartbeat; - heartbeat.message = "OK"; - heartbeat.count = 0; - - //记录 集群数据刷新的时间戳 - uint swarm_control_time[this->swarm_num_] = {0}; - //记录 未刷新的次数 - uint swarm_control_timeout_count[this->swarm_num_] = {0}; - //记录 无人机或无人车的时间戳 - uint time = 0; - uint time_count = 0; - while (true) - { - if (!this->is_heartbeat_ready_) - { - continue; - } - - sendMsgByTcp(encodeMsg(Send_Mode::TCP, heartbeat), udp_ip); - heartbeat.count++; - if (disconnect_num > try_connect_num) //跟地面站断联后的措施 - { - std::cout << "conenect ground station failed！" << std::endl; - //如果是集群模式 由集群模块触发降落 - if (this->swarm_num_ != 0 && this->swarm_control_ != NULL) - { - if (this->is_simulation_ == 0) - this->swarm_control_->communicationStatePub(false); - else - { - for (int i = 0; i < this->swarm_num_; i++) - { - this->swarm_control_->communicationStatePub(false, i); - } - } - } - //无人机 触发降落或者返航 - else if (this->uav_basic_ != NULL) - { - //触发降落 暂定 - struct UAVSetup uav_setup; - uav_setup.cmd = UAVSetup::CMD::SET_PX4_MODE; - uav_setup.px4_mode = "AUTO.LAND"; - //uav_setup.px4_mode = "AUTO.RTL"; //返航 - uav_setup.arming = false; - uav_setup.control_state = ""; - this->uav_basic_->uavSetupPub(uav_setup); - } - //无人车 停止小车 - else if (this->ugv_basic_ != NULL) - { - //停止小车 - struct RheaControl rhea_control; - rhea_control.Mode = RheaControl::Mode::Stop; - rhea_control.linear = 0; - rhea_control.angular = 0; - this->ugv_basic_->rheaControlPub(rhea_control); - } - //触发机制后 心跳准备标志置为false，停止心跳包的发送 再次接收到地面站指令激活 - this->is_heartbeat_ready_ = false; - } - - //无人机数据或者无人车数据是否超时 - if (this->uav_basic_ != NULL || this->ugv_basic_ != NULL) - { - uint time_stamp = 0; - if (this->uav_basic_ != NULL) - { - time_stamp = this->uav_basic_->getTimeStamp(); - } - else if (this->ugv_basic_ != NULL) - { - time_stamp = this->ugv_basic_->getTimeStamp(); - } - //拿单机状态时间戳进行比较 如果不相等说明数据在更新 - if (time != time_stamp) - { - time = time_stamp; - time_count = 0; - } - else //相等 数据未更新 - { - time_count++; - if (time_count > this->swarm_data_update_timeout_) - { - //反馈地面站 - struct TextInfo text_info; - text_info.MessageType = TextInfo::MessageTypeGrade::ERROR; - if (this->uav_basic_ != NULL) - text_info.Message = "UAV" + to_string(ROBOT_ID) + " data update timeout"; - else - text_info.Message = "UGV" + to_string(ROBOT_ID) + " data update timeout"; - text_info.sec = ros::Time::now().sec; - sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); - usleep(10); - } - } - } - - sleep(1); - } -} diff --git a/src/无人机端代码/Modules/communication/src/gimbal_basic_topic.cpp b/src/无人机端代码/Modules/communication/src/gimbal_basic_topic.cpp deleted file mode 100644 index a3da8bde..00000000 --- a/src/无人机端代码/Modules/communication/src/gimbal_basic_topic.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include "gimbal_basic_topic.hpp" - -GimbalBasic::GimbalBasic(ros::NodeHandle &nh,Communication *communication) -{ - nh.param("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("/detection/bbox_draw", 1000); - //【发布】吊舱控制 - this->gimbal_control_pub_ = nh.advertise("/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_); -} diff --git a/src/无人机端代码/Modules/communication/src/main.cpp b/src/无人机端代码/Modules/communication/src/main.cpp deleted file mode 100644 index 6126f805..00000000 --- a/src/无人机端代码/Modules/communication/src/main.cpp +++ /dev/null @@ -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; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/src/object_tracking_topic.cpp b/src/无人机端代码/Modules/communication/src/object_tracking_topic.cpp deleted file mode 100644 index 8f434bf9..00000000 --- a/src/无人机端代码/Modules/communication/src/object_tracking_topic.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "object_tracking_topic.hpp" - -ObjectTracking::ObjectTracking(ros::NodeHandle &nh,Communication *communication) -{ - nh.param("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); -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/communication/src/swarm_control_topic.cpp b/src/无人机端代码/Modules/communication/src/swarm_control_topic.cpp deleted file mode 100644 index 966f042a..00000000 --- a/src/无人机端代码/Modules/communication/src/swarm_control_topic.cpp +++ /dev/null @@ -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/swarm_command", 1000); - //【发布】连接是否失效 - this->communication_state_pub_ = nh.advertise("/uav" + std::to_string(id) + "/prometheus/communication_state", 10); - //【发布】所有无人机状态 - this->all_uav_state_pub_ = nh.advertise("/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("/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/all_uav_state", 1000); - //【发布】集群控制指令 - this->swarm_command_pub_ = nh.advertise("/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("ground_stationt_ip", udp_ip, "127.0.0.1"); - nh.param("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():"<communication_state_pub_.shutdown(); - // } - - // this->all_uav_state_pub_.shutdown(); - // this->swarm_command_pub_.shutdown(); -} diff --git a/src/无人机端代码/Modules/communication/src/uav_basic_topic.cpp b/src/无人机端代码/Modules/communication/src/uav_basic_topic.cpp deleted file mode 100644 index e5d533e5..00000000 --- a/src/无人机端代码/Modules/communication/src/uav_basic_topic.cpp +++ /dev/null @@ -1,147 +0,0 @@ -#include "uav_basic_topic.hpp" - -UAVBasic::UAVBasic() -{ - -} - -UAVBasic::UAVBasic(ros::NodeHandle &nh,int id,Communication *communication) -{ - nh.param("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("/uav" + std::to_string(id) + "/prometheus/command", 1); - //【发布】mavros接口调用指令(-> uav_control.cpp) - this->uav_setup_pub_ = nh.advertise("/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_; -} diff --git a/src/无人机端代码/Modules/communication/src/ugv_basic_topic.cpp b/src/无人机端代码/Modules/communication/src/ugv_basic_topic.cpp deleted file mode 100644 index 1dc22d23..00000000 --- a/src/无人机端代码/Modules/communication/src/ugv_basic_topic.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "ugv_basic_topic.hpp" - -UGVBasic::UGVBasic(ros::NodeHandle &nh,Communication *communication) -{ - this->communication_ = communication; - nh.param("ground_stationt_ip", udp_ip, "127.0.0.1"); - nh.param("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("/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_; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/CMakeLists.txt b/src/无人机端代码/Modules/ego_planner_swarm/CMakeLists.txt deleted file mode 100644 index 66dd650a..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/CMakeLists.txt b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/CMakeLists.txt deleted file mode 100644 index e61c145f..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/CMakeLists.txt +++ /dev/null @@ -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} - ) diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/bspline_optimizer.h b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/bspline_optimizer.h deleted file mode 100644 index 838d62ec..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/bspline_optimizer.h +++ /dev/null @@ -1,213 +0,0 @@ -#ifndef _BSPLINE_OPTIMIZER_H_ -#define _BSPLINE_OPTIMIZER_H_ - -#include -#include -#include -#include -#include -#include -#include "bspline_opt/lbfgs.hpp" -#include - -// 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> base_point; // The point at the statrt of the direction vector (collision point) - std::vector> direction; // Direction vector, must be normalized. - std::vector flag_temp; // A flag that used in many places. Initialize it everytime before using it. - // std::vector 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 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 &guide_pt); - void setWaypoints(const vector &waypts, - const vector &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 ref_pts_; - - std::vector distinctiveTrajs(vector> segments); - std::vector> 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 guide_pts_; // geometric guiding path points, N-6 - vector waypoints_; // waypts constraints - vector 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 &x, std::vector &grad, void *func_data); - void combineCost(const std::vector &x, vector &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 Ptr; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - -} // namespace ego_planner -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h deleted file mode 100644 index 01a5a716..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef _GRADIENT_DESCENT_OPT_H_ -#define _GRADIENT_DESCENT_OPT_H_ - -#include -#include -#include - -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 diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/lbfgs.hpp b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/lbfgs.hpp deleted file mode 100644 index 484c4b64..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/lbfgs.hpp +++ /dev/null @@ -1,1451 +0,0 @@ -#ifndef LBFGS_HPP -#define LBFGS_HPP - -#include -#include -#include -#include -#include - -namespace lbfgs -{ - // ----------------------- Data Type Part ----------------------- - - /** - * Return values of lbfgs_optimize(). - * - * Roughly speaking, a negative value indicates an error. - */ - - enum - { - /** L-BFGS reaches convergence. */ - LBFGS_CONVERGENCE = 0, - /** L-BFGS satisfies stopping criteria. */ - LBFGS_STOP, - /** The initial variables already minimize the objective function. */ - LBFGS_ALREADY_MINIMIZED, - - /** Unknown error. */ - LBFGSERR_UNKNOWNERROR = -1024, - /** Logic error. */ - LBFGSERR_LOGICERROR, - /** The minimization process has been canceled. */ - LBFGSERR_CANCELED, - /** Invalid number of variables specified. */ - LBFGSERR_INVALID_N, - /** Invalid parameter lbfgs_parameter_t::mem_size specified. */ - LBFGSERR_INVALID_MEMSIZE, - /** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */ - LBFGSERR_INVALID_GEPSILON, - /** Invalid parameter lbfgs_parameter_t::past specified. */ - LBFGSERR_INVALID_TESTPERIOD, - /** Invalid parameter lbfgs_parameter_t::delta specified. */ - LBFGSERR_INVALID_DELTA, - /** Invalid parameter lbfgs_parameter_t::min_step specified. */ - LBFGSERR_INVALID_MINSTEP, - /** Invalid parameter lbfgs_parameter_t::max_step specified. */ - LBFGSERR_INVALID_MAXSTEP, - /** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */ - LBFGSERR_INVALID_FDECCOEFF, - /** Invalid parameter lbfgs_parameter_t::wolfe specified. */ - LBFGSERR_INVALID_SCURVCOEFF, - /** Invalid parameter lbfgs_parameter_t::xtol specified. */ - LBFGSERR_INVALID_XTOL, - /** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */ - LBFGSERR_INVALID_MAXLINESEARCH, - /** The line-search step went out of the interval of uncertainty. */ - LBFGSERR_OUTOFINTERVAL, - /** A logic error occurred; alternatively, the interval of uncertainty - became too small. */ - LBFGSERR_INCORRECT_TMINMAX, - /** A rounding error occurred; alternatively, no line-search step - satisfies the sufficient decrease and curvature conditions. */ - LBFGSERR_ROUNDING_ERROR, - /** The line-search step became smaller than lbfgs_parameter_t::min_step. */ - LBFGSERR_MINIMUMSTEP, - /** The line-search step became larger than lbfgs_parameter_t::max_step. */ - LBFGSERR_MAXIMUMSTEP, - /** The line-search routine reaches the maximum number of evaluations. */ - LBFGSERR_MAXIMUMLINESEARCH, - /** The algorithm routine reaches the maximum number of iterations. */ - LBFGSERR_MAXIMUMITERATION, - /** Relative width of the interval of uncertainty is at most - lbfgs_parameter_t::xtol. */ - LBFGSERR_WIDTHTOOSMALL, - /** A logic error (negative line-search step) occurred. */ - LBFGSERR_INVALIDPARAMETERS, - /** The current search direction increases the objective function value. */ - LBFGSERR_INCREASEGRADIENT, - }; - - /** - * L-BFGS optimization parameters. - * Call lbfgs_load_default_parameters() function to initialize parameters to the - * default values. - */ - struct lbfgs_parameter_t - { - /** - * The number of corrections to approximate the inverse hessian matrix. - * The L-BFGS routine stores the computation results of previous m - * iterations to approximate the inverse hessian matrix of the current - * iteration. This parameter controls the size of the limited memories - * (corrections). The default value is 6. Values less than 3 are - * not recommended. Large values will result in excessive computing time. - */ - int mem_size; - - /** - * Epsilon for grad norm convergence test. - * This parameter determines the accuracy with which the solution is to - * be found. A minimization terminates when - * ||g|| < g_epsilon * max(1, ||x||), - * where ||.|| denotes the Euclidean (L2) norm. The default value is 1e-5. - */ - double g_epsilon; - - /** - * Distance for delta-based convergence test. - * This parameter determines the distance, in iterations, to compute - * the rate of decrease of the objective function. If the value of this - * parameter is zero, the library does not perform the delta-based - * convergence test. The default value is 0. - */ - int past; - - /** - * Delta for convergence test. - * This parameter determines the minimum rate of decrease of the - * objective function. The library stops iterations when the - * following condition is met: - * (f' - f) / f < delta, - * where f' is the objective value of past iterations ago, and f is - * the objective value of the current iteration. - * The default value is 1e-5. - */ - double delta; - - /** - * The maximum number of iterations. - * The lbfgs_optimize() function terminates an optimization process with - * ::LBFGSERR_MAXIMUMITERATION status code when the iteration count - * exceedes this parameter. Setting this parameter to zero continues an - * optimization process until a convergence or error. The default value - * is 0. - */ - int max_iterations; - - /** - * The maximum number of trials for the line search. - * This parameter controls the number of function and gradients evaluations - * per iteration for the line search routine. The default value is 40. - */ - int max_linesearch; - - /** - * The minimum step of the line search routine. - * The default value is 1e-20. This value need not be modified unless - * the exponents are too large for the machine being used, or unless the - * problem is extremely badly scaled (in which case the exponents should - * be increased). - */ - double min_step; - - /** - * The maximum step of the line search. - * The default value is 1e+20. This value need not be modified unless - * the exponents are too large for the machine being used, or unless the - * problem is extremely badly scaled (in which case the exponents should - * be increased). - */ - double max_step; - - /** - * A parameter to control the accuracy of the line search routine. - * The default value is 1e-4. This parameter should be greater - * than zero and smaller than 0.5. - */ - double f_dec_coeff; - - /** - * A parameter to control the accuracy of the line search routine. - * The default value is 0.9. If the function and gradient - * evaluations are inexpensive with respect to the cost of the - * iteration (which is sometimes the case when solving very large - * problems) it may be advantageous to set this parameter to a small - * value. A typical small value is 0.1. This parameter shuold be - * greater than the f_dec_coeff parameter (1e-4) - * and smaller than 1.0. - */ - double s_curv_coeff; - - /** - * The machine precision for floating-point values. - * This parameter must be a positive value set by a client program to - * estimate the machine precision. The line search routine will terminate - * with the status code (::LBFGSERR_ROUNDING_ERROR) if the relative width - * of the interval of uncertainty is less than this parameter. - */ - double xtol; - }; - - /** - * Callback interface to provide objective function and gradient evaluations. - * - * The lbfgs_optimize() function call this function to obtain the values of objective - * function and its gradients when needed. A client program must implement - * this function to evaluate the values of the objective function and its - * gradients, given current values of variables. - * - * @param instance The user data sent for lbfgs_optimize() function by the client. - * @param x The current values of variables. - * @param g The gradient vector. The callback function must compute - * the gradient values for the current variables. - * @param n The number of variables. - * @retval double The value of the objective function for the current - * variables. - */ - typedef double (*lbfgs_evaluate_t)(void *instance, - const double *x, - double *g, - const int n); - - /** - * Callback interface to provide an upper bound at the beginning of current linear search. - * - * The lbfgs_optimize() function call this function to obtain the values of the - * upperbound of the stepsize to search in, provided with the beginning values of - * variables before the linear search, and the current step vector (can be descent direction). - * A client program can implement this function for more efficient linesearch. - * If it is not used, just set it NULL or nullptr. - * - * @param instance The user data sent for lbfgs_optimize() function by the client. - * @param xp The values of variables before current line search. - * @param d The step vector. It can be the descent direction. - * @param n The number of variables. - * @retval double The upperboud of the step in current line search routine, - * such that stpbound*d is the maximum reasonable step. - */ - typedef double (*lbfgs_stepbound_t)(void *instance, - const double *xp, - const double *d, - const int n); - - /** - * Callback interface to receive the progress of the optimization process. - * - * The lbfgs_optimize() function call this function for each iteration. Implementing - * this function, a client program can store or display the current progress - * of the optimization process. If it is not used, just set it NULL or nullptr. - * - * @param instance The user data sent for lbfgs_optimize() function by the client. - * @param x The current values of variables. - * @param g The current gradient values of variables. - * @param fx The current value of the objective function. - * @param xnorm The Euclidean norm of the variables. - * @param gnorm The Euclidean norm of the gradients. - * @param step The line-search step used for this iteration. - * @param n The number of variables. - * @param k The iteration count. - * @param ls The number of evaluations called for this iteration. - * @retval int Zero to continue the optimization process. Returning a - * non-zero value will cancel the optimization process. - */ - typedef int (*lbfgs_progress_t)(void *instance, - const double *x, - const double *g, - const double fx, - const double xnorm, - const double gnorm, - const double step, - int n, - int k, - int ls); - - /** - * Callback data struct - */ - - struct callback_data_t - { - int n; - void *instance; - lbfgs_evaluate_t proc_evaluate; - lbfgs_stepbound_t proc_stepbound; - lbfgs_progress_t proc_progress; - }; - - /** - * Iteration data struct - */ - struct iteration_data_t - { - double alpha; - double *s; /* [n] */ - double *y; /* [n] */ - double ys; /* vecdot(y, s) */ - }; - - // ----------------------- Arithmetic Part ----------------------- - -/** - * Define the local variables for computing minimizers. - */ -#define USES_MINIMIZER_LBFGS \ - double a, d, gamm, theta, p, q, r, s; - -/** - * Find a minimizer of an interpolated cubic function. - * @param cm The minimizer of the interpolated cubic. - * @param u The value of one point, u. - * @param fu The value of f(u). - * @param du The value of f'(u). - * @param v The value of another point, v. - * @param fv The value of f(v). - * @param du The value of f'(v). - */ -#define CUBIC_MINIMIZER_LBFGS(cm, u, fu, du, v, fv, dv) \ - d = (v) - (u); \ - theta = ((fu) - (fv)) * 3 / d + (du) + (dv); \ - p = fabs(theta); \ - q = fabs(du); \ - r = fabs(dv); \ - s = p >= q ? p : q; \ - s = s >= r ? s : r; \ - /* gamm = s*sqrt((theta/s)**2 - (du/s) * (dv/s)) */ \ - a = theta / s; \ - gamm = s * sqrt(a * a - ((du) / s) * ((dv) / s)); \ - if ((v) < (u)) \ - gamm = -gamm; \ - p = gamm - (du) + theta; \ - q = gamm - (du) + gamm + (dv); \ - r = p / q; \ - (cm) = (u) + r * d; - -/** - * Find a minimizer of an interpolated cubic function. - * @param cm The minimizer of the interpolated cubic. - * @param u The value of one point, u. - * @param fu The value of f(u). - * @param du The value of f'(u). - * @param v The value of another point, v. - * @param fv The value of f(v). - * @param du The value of f'(v). - * @param xmin The maximum value. - * @param xmin The minimum value. - */ -#define CUBIC_MINIMIZER2_LBFGS(cm, u, fu, du, v, fv, dv, xmin, xmax) \ - d = (v) - (u); \ - theta = ((fu) - (fv)) * 3 / d + (du) + (dv); \ - p = fabs(theta); \ - q = fabs(du); \ - r = fabs(dv); \ - s = p >= q ? p : q; \ - s = s >= r ? s : r; \ - /* gamm = s*sqrt((theta/s)**2 - (du/s) * (dv/s)) */ \ - a = theta / s; \ - gamm = a * a - ((du) / s) * ((dv) / s); \ - gamm = gamm > 0 ? s * sqrt(gamm) : 0; \ - if ((u) < (v)) \ - gamm = -gamm; \ - p = gamm - (dv) + theta; \ - q = gamm - (dv) + gamm + (du); \ - r = p / q; \ - if (r < 0. && gamm != 0.) \ - { \ - (cm) = (v)-r * d; \ - } \ - else if (a < 0) \ - { \ - (cm) = (xmax); \ - } \ - else \ - { \ - (cm) = (xmin); \ - } - -/** - * Find a minimizer of an interpolated quadratic function. - * @param qm The minimizer of the interpolated quadratic. - * @param u The value of one point, u. - * @param fu The value of f(u). - * @param du The value of f'(u). - * @param v The value of another point, v. - * @param fv The value of f(v). - */ -#define QUARD_MINIMIZER_LBFGS(qm, u, fu, du, v, fv) \ - a = (v) - (u); \ - (qm) = (u) + (du) / (((fu) - (fv)) / a + (du)) / 2 * a; - -/** - * Find a minimizer of an interpolated quadratic function. - * @param qm The minimizer of the interpolated quadratic. - * @param u The value of one point, u. - * @param du The value of f'(u). - * @param v The value of another point, v. - * @param dv The value of f'(v). - */ -#define QUARD_MINIMIZER2_LBFGS(qm, u, du, v, dv) \ - a = (u) - (v); \ - (qm) = (v) + (dv) / ((dv) - (du)) * a; - - inline void *vecalloc(size_t size) - { - void *memblock = malloc(size); - if (memblock) - { - memset(memblock, 0, size); - } - return memblock; - } - - inline void vecfree(void *memblock) - { - free(memblock); - } - - inline void veccpy(double *y, const double *x, const int n) - { - memcpy(y, x, sizeof(double) * n); - } - - inline void vecncpy(double *y, const double *x, const int n) - { - int i; - - for (i = 0; i < n; ++i) - { - y[i] = -x[i]; - } - } - - inline void vecadd(double *y, const double *x, const double c, const int n) - { - int i; - - for (i = 0; i < n; ++i) - { - y[i] += c * x[i]; - } - } - - inline void vecdiff(double *z, const double *x, const double *y, const int n) - { - int i; - - for (i = 0; i < n; ++i) - { - z[i] = x[i] - y[i]; - } - } - - inline void vecscale(double *y, const double c, const int n) - { - int i; - - for (i = 0; i < n; ++i) - { - y[i] *= c; - } - } - - inline void vecdot(double *s, const double *x, const double *y, const int n) - { - int i; - *s = 0.; - for (i = 0; i < n; ++i) - { - *s += x[i] * y[i]; - } - } - - inline void vec2norm(double *s, const double *x, const int n) - { - vecdot(s, x, x, n); - *s = (double)sqrt(*s); - } - - inline void vec2norminv(double *s, const double *x, const int n) - { - vec2norm(s, x, n); - *s = (double)(1.0 / *s); - } - - // ----------------------- L-BFGS Part ----------------------- - - /** - * Update a safeguarded trial value and interval for line search. - * - * The parameter x represents the step with the least function value. - * The parameter t represents the current step. This function assumes - * that the derivative at the point of x in the direction of the step. - * If the bracket is set to true, the minimizer has been bracketed in - * an interval of uncertainty with endpoints between x and y. - * - * @param x The pointer to the value of one endpoint. - * @param fx The pointer to the value of f(x). - * @param dx The pointer to the value of f'(x). - * @param y The pointer to the value of another endpoint. - * @param fy The pointer to the value of f(y). - * @param dy The pointer to the value of f'(y). - * @param t The pointer to the value of the trial value, t. - * @param ft The pointer to the value of f(t). - * @param dt The pointer to the value of f'(t). - * @param tmin The minimum value for the trial value, t. - * @param tmax The maximum value for the trial value, t. - * @param brackt The pointer to the predicate if the trial value is - * bracketed. - * @retval int Status value. Zero indicates a normal termination. - * - * @see - * Jorge J. More and David J. Thuente. Line search algorithm with - * guaranteed sufficient decrease. ACM Transactions on Mathematical - * Software (TOMS), Vol 20, No 3, pp. 286-307, 1994. - */ - inline int update_trial_interval(double *x, - double *fx, - double *dx, - double *y, - double *fy, - double *dy, - double *t, - double *ft, - double *dt, - const double tmin, - const double tmax, - int *brackt) - { - int bound; - int dsign = *dt * (*dx / fabs(*dx)) < 0.; - double mc; /* minimizer of an interpolated cubic. */ - double mq; /* minimizer of an interpolated quadratic. */ - double newt; /* new trial value. */ - USES_MINIMIZER_LBFGS; /* for CUBIC_MINIMIZER and QUARD_MINIMIZER. */ - - /* Check the input parameters for errors. */ - if (*brackt) - { - if (*t <= (*x <= *y ? *x : *y) || (*x >= *y ? *x : *y) <= *t) - { - /* The trival value t is out of the interval. */ - return LBFGSERR_OUTOFINTERVAL; - } - if (0. <= *dx * (*t - *x)) - { - /* The function must decrease from x. */ - return LBFGSERR_INCREASEGRADIENT; - } - if (tmax < tmin) - { - /* Incorrect tmin and tmax specified. */ - return LBFGSERR_INCORRECT_TMINMAX; - } - } - - /* - Trial value selection. - */ - if (*fx < *ft) - { - /* - Case 1: a higher function value. - The minimum is brackt. If the cubic minimizer is closer - to x than the quadratic one, the cubic one is taken, else - the average of the minimizers is taken. - */ - *brackt = 1; - bound = 1; - CUBIC_MINIMIZER_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt); - QUARD_MINIMIZER_LBFGS(mq, *x, *fx, *dx, *t, *ft); - if (fabs(mc - *x) < fabs(mq - *x)) - { - newt = mc; - } - else - { - newt = mc + 0.5 * (mq - mc); - } - } - else if (dsign) - { - /* - Case 2: a lower function value and derivatives of - opposite sign. The minimum is brackt. If the cubic - minimizer is closer to x than the quadratic (secant) one, - the cubic one is taken, else the quadratic one is taken. - */ - *brackt = 1; - bound = 0; - CUBIC_MINIMIZER_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt); - QUARD_MINIMIZER2_LBFGS(mq, *x, *dx, *t, *dt); - if (fabs(mc - *t) > fabs(mq - *t)) - { - newt = mc; - } - else - { - newt = mq; - } - } - else if (fabs(*dt) < fabs(*dx)) - { - /* - Case 3: a lower function value, derivatives of the - same sign, and the magnitude of the derivative decreases. - The cubic minimizer is only used if the cubic tends to - infinity in the direction of the minimizer or if the minimum - of the cubic is beyond t. Otherwise the cubic minimizer is - defined to be either tmin or tmax. The quadratic (secant) - minimizer is also computed and if the minimum is brackt - then the the minimizer closest to x is taken, else the one - farthest away is taken. - */ - bound = 1; - CUBIC_MINIMIZER2_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt, tmin, tmax); - QUARD_MINIMIZER2_LBFGS(mq, *x, *dx, *t, *dt); - if (*brackt) - { - if (fabs(*t - mc) < fabs(*t - mq)) - { - newt = mc; - } - else - { - newt = mq; - } - } - else - { - if (fabs(*t - mc) > fabs(*t - mq)) - { - newt = mc; - } - else - { - newt = mq; - } - } - } - else - { - /* - Case 4: a lower function value, derivatives of the - same sign, and the magnitude of the derivative does - not decrease. If the minimum is not brackt, the step - is either tmin or tmax, else the cubic minimizer is taken. - */ - bound = 0; - if (*brackt) - { - CUBIC_MINIMIZER_LBFGS(newt, *t, *ft, *dt, *y, *fy, *dy); - } - else if (*x < *t) - { - newt = tmax; - } - else - { - newt = tmin; - } - } - - /* - Update the interval of uncertainty. This update does not - depend on the new step or the case analysis above. - - - Case a: if f(x) < f(t), - x <- x, y <- t. - - Case b: if f(t) <= f(x) && f'(t)*f'(x) > 0, - x <- t, y <- y. - - Case c: if f(t) <= f(x) && f'(t)*f'(x) < 0, - x <- t, y <- x. - */ - if (*fx < *ft) - { - /* Case a */ - *y = *t; - *fy = *ft; - *dy = *dt; - } - else - { - /* Case c */ - if (dsign) - { - *y = *x; - *fy = *fx; - *dy = *dx; - } - /* Cases b and c */ - *x = *t; - *fx = *ft; - *dx = *dt; - } - - /* Clip the new trial value in [tmin, tmax]. */ - if (tmax < newt) - newt = tmax; - if (newt < tmin) - newt = tmin; - - /* - Redefine the new trial value if it is close to the upper bound - of the interval. - */ - if (*brackt && bound) - { - mq = *x + 0.66 * (*y - *x); - if (*x < *y) - { - if (mq < newt) - newt = mq; - } - else - { - if (newt < mq) - newt = mq; - } - } - - /* Return the new trial value. */ - *t = newt; - return 0; - } - - inline int line_search_morethuente(int n, - double *x, - double *f, - double *g, - double *s, - double *stp, - const double *xp, - const double *gp, - const double *stpmin, - const double *stpmax, - callback_data_t *cd, - const lbfgs_parameter_t *param) - { - int count = 0; - int brackt, stage1, uinfo = 0; - double dg; - double stx, fx, dgx; - double sty, fy, dgy; - double fxm, dgxm, fym, dgym, fm, dgm; - double finit, ftest1, dginit, dgtest; - double width, prev_width; - double stmin, stmax; - - /* Check the input parameters for errors. */ - if (*stp <= 0.) - { - return LBFGSERR_INVALIDPARAMETERS; - } - - /* Compute the initial gradient in the search direction. */ - vecdot(&dginit, g, s, n); - - /* Make sure that s points to a descent direction. */ - if (0 < dginit) - { - return LBFGSERR_INCREASEGRADIENT; - } - - /* Initialize local variables. */ - brackt = 0; - stage1 = 1; - finit = *f; - dgtest = param->f_dec_coeff * dginit; - width = *stpmax - *stpmin; - prev_width = 2.0 * width; - - /* - The variables stx, fx, dgx contain the values of the step, - function, and directional derivative at the best step. - The variables sty, fy, dgy contain the value of the step, - function, and derivative at the other endpoint of - the interval of uncertainty. - The variables stp, f, dg contain the values of the step, - function, and derivative at the current step. - */ - stx = sty = 0.; - fx = fy = finit; - dgx = dgy = dginit; - - for (;;) - { - /* Report the progress. */ - if (cd->proc_progress) - { - double xnorm; - double gnorm; - vec2norm(&xnorm, x, n); - vec2norm(&gnorm, g, n); - if (cd->proc_progress(cd->instance, x, g, fx, xnorm, gnorm, *stp, cd->n, 0, 0)) - { - return LBFGSERR_CANCELED; - } - } - - /* - Set the minimum and maximum steps to correspond to the - present interval of uncertainty. - */ - if (brackt) - { - stmin = stx <= sty ? stx : sty; - stmax = stx >= sty ? stx : sty; - } - else - { - stmin = stx; - stmax = *stp + 4.0 * (*stp - stx); - } - - /* Clip the step in the range of [stpmin, stpmax]. */ - if (*stp < *stpmin) - *stp = *stpmin; - if (*stpmax < *stp) - *stp = *stpmax; - - /* - If an unusual termination is to occur then let - stp be the lowest point obtained so far. - */ - if ((brackt && ((*stp <= stmin || stmax <= *stp) || param->max_linesearch <= count + 1 || uinfo != 0)) || (brackt && (stmax - stmin <= param->xtol * stmax))) - { - *stp = stx; - } - - /* - Compute the current value of x: - x <- x + (*stp) * s. - */ - veccpy(x, xp, n); - vecadd(x, s, *stp, n); - - /* Evaluate the function and gradient values. */ - *f = cd->proc_evaluate(cd->instance, x, g, cd->n); - vecdot(&dg, g, s, n); - - ftest1 = finit + *stp * dgtest; - ++count; - - /* Test for errors and convergence. */ - if (brackt && ((*stp <= stmin || stmax <= *stp) || uinfo != 0)) - { - /* Rounding errors prevent further progress. */ - return LBFGSERR_ROUNDING_ERROR; - } - if (*stp == *stpmax && *f <= ftest1 && dg <= dgtest) - { - /* The step is the maximum value. */ - return LBFGSERR_MAXIMUMSTEP; - } - if (*stp == *stpmin && (ftest1 < *f || dgtest <= dg)) - { - /* The step is the minimum value. */ - return LBFGSERR_MINIMUMSTEP; - } - if (brackt && (stmax - stmin) <= param->xtol * stmax) - { - /* Relative width of the interval of uncertainty is at most xtol. */ - return LBFGSERR_WIDTHTOOSMALL; - } - if (param->max_linesearch <= count) - { - /* Maximum number of iteration. */ - return LBFGSERR_MAXIMUMLINESEARCH; - } - if (*f <= ftest1 && fabs(dg) <= param->s_curv_coeff * (-dginit)) - { - /* The sufficient decrease condition and the strong curvature condition hold. */ - return count; - } - - /* - In the first stage we seek a step for which the modified - function has a nonpositive value and nonnegative derivative. - */ - if (stage1 && *f <= ftest1 && - (param->f_dec_coeff <= param->s_curv_coeff ? param->f_dec_coeff : param->s_curv_coeff) * dginit <= dg) - { - stage1 = 0; - } - - /* - A modified function is used to predict the step only if - we have not obtained a step for which the modified - function has a nonpositive function value and nonnegative - derivative, and if a lower function value has been - obtained but the decrease is not sufficient. - */ - if (stage1 && ftest1 < *f && *f <= fx) - { - /* Define the modified function and derivative values. */ - fm = *f - *stp * dgtest; - fxm = fx - stx * dgtest; - fym = fy - sty * dgtest; - dgm = dg - dgtest; - dgxm = dgx - dgtest; - dgym = dgy - dgtest; - - /* - Call update_trial_interval() to update the interval of - uncertainty and to compute the new step. - */ - uinfo = update_trial_interval( - &stx, &fxm, &dgxm, - &sty, &fym, &dgym, - stp, &fm, &dgm, - stmin, stmax, &brackt); - - /* Reset the function and gradient values for f. */ - fx = fxm + stx * dgtest; - fy = fym + sty * dgtest; - dgx = dgxm + dgtest; - dgy = dgym + dgtest; - } - else - { - /* - Call update_trial_interval() to update the interval of - uncertainty and to compute the new step. - */ - uinfo = update_trial_interval( - &stx, &fx, &dgx, - &sty, &fy, &dgy, - stp, f, &dg, - stmin, stmax, &brackt); - } - - /* - Force a sufficient decrease in the interval of uncertainty. - */ - if (brackt) - { - if (0.66 * prev_width <= fabs(sty - stx)) - { - *stp = stx + 0.5 * (sty - stx); - } - prev_width = width; - width = fabs(sty - stx); - } - } - - return LBFGSERR_LOGICERROR; - } - - /** - * Default L-BFGS parameters. - */ - static const lbfgs_parameter_t _default_param = { - 8, - 1e-5, - 0, - 1e-5, - 0, - 40, - 1e-20, - 1e20, - 1e-4, - 0.9, - 1.0e-16, - }; - - /** - * Initialize L-BFGS parameters to the default values. - * - * Call this function to fill a parameter structure with the default values - * and overwrite parameter values if necessary. - * - * @param param The pointer to the parameter structure. - */ - inline void lbfgs_load_default_parameters(lbfgs_parameter_t *param) - { - memcpy(param, &_default_param, sizeof(*param)); - } - - /** - * Start a L-BFGS optimization. - * A user must implement a function compatible with ::lbfgs_evaluate_t (evaluation - * callback) and pass the pointer to the callback function to lbfgs_optimize() - * arguments. Similarly, a user can implement a function compatible with - * ::lbfgs_stepbound_t to provide an external upper bound for stepsize, and - * ::lbfgs_progress_t (progress callback) to obtain the current progress - * (e.g., variables, function value, ||G||, etc) and to cancel the iteration - * process if necessary. Implementation of the stepbound and the progress callback - * is optional: a user can pass NULL if progress notification is not necessary. - * - * This algorithm terminates an optimization - * when: - * - * ||G|| < g_epsilon \cdot \max(1, ||x||) . - * - * In this formula, ||.|| denotes the Euclidean norm. - * - * @param n The number of variables. - * @param x The array of variables. A client program can set - * default values for the optimization and receive the - * optimization result through this array. - * @param ptr_fx The pointer to the variable that receives the final - * value of the objective function for the variables. - * This argument can be set to NULL if the final - * value of the objective function is unnecessary. - * @param proc_evaluate The callback function to provide function and - * gradient evaluations given a current values of - * variables. A client program must implement a - * callback function compatible with - * lbfgs_evaluate_t and pass the pointer to the - * callback function. - * @param proc_stepbound The callback function to provide values of the - * upperbound of the stepsize to search in, provided - * with the beginning values of variables before the - * linear search, and the current step vector (can - * be negative gradient). A client program can implement - * this function for more efficient linesearch. If it is - * not used, just set it NULL or nullptr. - * @param proc_progress The callback function to receive the progress - * (the number of iterations, the current value of - * the objective function) of the minimization - * process. This argument can be set to NULL if - * a progress report is unnecessary. - * @param instance A user data for the client program. The callback - * functions will receive the value of this argument. - * @param param The pointer to a structure representing parameters for - * L-BFGS optimization. A client program can set this - * parameter to NULL to use the default parameters. - * Call lbfgs_load_default_parameters() function to - * fill a structure with the default values. - * @retval int The status code. This function returns zero if the - * minimization process terminates without an error. A - * non-zero value indicates an error. - */ - inline int lbfgs_optimize(int n, - double *x, - double *ptr_fx, - lbfgs_evaluate_t proc_evaluate, - lbfgs_stepbound_t proc_stepbound, - lbfgs_progress_t proc_progress, - void *instance, - lbfgs_parameter_t *_param) - { - int ret; - int i, j, k, ls, end, bound; - double step; - int loop; - double step_min, step_max; - - /* Constant parameters and their default values. */ - lbfgs_parameter_t param = (_param != NULL) ? (*_param) : _default_param; - const int m = param.mem_size; - - double *xp = NULL; - double *g = NULL, *gp = NULL; - double *d = NULL, *pf = NULL; - iteration_data_t *lm = NULL, *it = NULL; - double ys, yy; - double xnorm, gnorm, beta; - double fx = 0.; - double rate = 0.; - - /* Construct a callback data. */ - callback_data_t cd; - cd.n = n; - cd.instance = instance; - cd.proc_evaluate = proc_evaluate; - cd.proc_stepbound = proc_stepbound; - cd.proc_progress = proc_progress; - - /* Check the input parameters for errors. */ - if (n <= 0) - { - return LBFGSERR_INVALID_N; - } - if (m <= 0) - { - return LBFGSERR_INVALID_MEMSIZE; - } - if (param.g_epsilon < 0.) - { - return LBFGSERR_INVALID_GEPSILON; - } - if (param.past < 0) - { - return LBFGSERR_INVALID_TESTPERIOD; - } - if (param.delta < 0.) - { - return LBFGSERR_INVALID_DELTA; - } - if (param.min_step < 0.) - { - return LBFGSERR_INVALID_MINSTEP; - } - if (param.max_step < param.min_step) - { - return LBFGSERR_INVALID_MAXSTEP; - } - if (param.f_dec_coeff < 0.) - { - return LBFGSERR_INVALID_FDECCOEFF; - } - if (param.s_curv_coeff <= param.f_dec_coeff || 1. <= param.s_curv_coeff) - { - return LBFGSERR_INVALID_SCURVCOEFF; - } - if (param.xtol < 0.) - { - return LBFGSERR_INVALID_XTOL; - } - if (param.max_linesearch <= 0) - { - return LBFGSERR_INVALID_MAXLINESEARCH; - } - - /* Allocate working space. */ - xp = (double *)vecalloc(n * sizeof(double)); - g = (double *)vecalloc(n * sizeof(double)); - gp = (double *)vecalloc(n * sizeof(double)); - d = (double *)vecalloc(n * sizeof(double)); - - /* Allocate limited memory storage. */ - lm = (iteration_data_t *)vecalloc(m * sizeof(iteration_data_t)); - - /* Initialize the limited memory. */ - for (i = 0; i < m; ++i) - { - it = &lm[i]; - it->alpha = 0; - it->ys = 0; - it->s = (double *)vecalloc(n * sizeof(double)); - it->y = (double *)vecalloc(n * sizeof(double)); - } - - /* Allocate an array for storing previous values of the objective function. */ - if (0 < param.past) - { - pf = (double *)vecalloc(param.past * sizeof(double)); - } - - /* Evaluate the function value and its gradient. */ - fx = cd.proc_evaluate(cd.instance, x, g, cd.n); - - /* Store the initial value of the objective function. */ - if (pf != NULL) - { - pf[0] = fx; - } - - /* - Compute the direction; - we assume the initial hessian matrix H_0 as the identity matrix. - */ - vecncpy(d, g, n); - - /* - Make sure that the initial variables are not a minimizer. - */ - vec2norm(&xnorm, x, n); - vec2norm(&gnorm, g, n); - - if (xnorm < 1.0) - xnorm = 1.0; - if (gnorm / xnorm <= param.g_epsilon) - { - ret = LBFGS_ALREADY_MINIMIZED; - } - else - { - /* Compute the initial step: - step = 1.0 / sqrt(vecdot(d, d, n)) - */ - vec2norminv(&step, d, n); - - k = 1; - end = 0; - loop = 1; - - while (loop == 1) - { - /* Store the current position and gradient vectors. */ - veccpy(xp, x, n); - veccpy(gp, g, n); - - // If the step bound can be provied dynamically, then apply it. - step_min = param.min_step; - step_max = param.max_step; - if (cd.proc_stepbound) - { - step_max = cd.proc_stepbound(cd.instance, xp, d, cd.n); - step_max = step_max < param.max_step ? step_max : param.max_step; - if (step >= step_max) - step = step_max / 2.0; - } - - /* Search for an optimal step. */ - ls = line_search_morethuente(n, x, &fx, g, d, &step, xp, gp, &step_min, &step_max, &cd, ¶m); - - if (ls < 0) - { - /* Revert to the previous point. */ - veccpy(x, xp, n); - veccpy(g, gp, n); - ret = ls; - loop = 0; - continue; - } - - /* Compute x and g norms. */ - vec2norm(&xnorm, x, n); - vec2norm(&gnorm, g, n); - - // /* Report the progress. */ - // if (cd.proc_progress) - // { - // if ((ret = cd.proc_progress(cd.instance, x, g, fx, xnorm, gnorm, step, cd.n, k, ls))) - // { - // loop = 0; - // continue; - // } - // } - - /* - Convergence test. - The criterion is given by the following formula: - |g(x)| / \max(1, |x|) < g_epsilon - */ - if (xnorm < 1.0) - xnorm = 1.0; - if (gnorm / xnorm <= param.g_epsilon) - { - /* Convergence. */ - ret = LBFGS_CONVERGENCE; - break; - } - - /* - Test for stopping criterion. - The criterion is given by the following formula: - |(f(past_x) - f(x))| / f(x) < \delta - */ - if (pf != NULL) - { - /* We don't test the stopping criterion while k < past. */ - if (param.past <= k) - { - /* Compute the relative improvement from the past. */ - rate = (pf[k % param.past] - fx) / fx; - - /* The stopping criterion. */ - if (fabs(rate) < param.delta) - { - ret = LBFGS_STOP; - break; - } - } - - /* Store the current value of the objective function. */ - pf[k % param.past] = fx; - } - - if (param.max_iterations != 0 && param.max_iterations < k + 1) - { - /* Maximum number of iterations. */ - ret = LBFGSERR_MAXIMUMITERATION; - break; - } - - /* - Update vectors s and y: - s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}. - y_{k+1} = g_{k+1} - g_{k}. - */ - it = &lm[end]; - vecdiff(it->s, x, xp, n); - vecdiff(it->y, g, gp, n); - - /* - Compute scalars ys and yy: - ys = y^t \cdot s = 1 / \rho. - yy = y^t \cdot y. - Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor). - */ - vecdot(&ys, it->y, it->s, n); - vecdot(&yy, it->y, it->y, n); - it->ys = ys; - - /* - Recursive formula to compute dir = -(H \cdot g). - This is described in page 779 of: - Jorge Nocedal. - Updating Quasi-Newton Matrices with Limited Storage. - Mathematics of Computation, Vol. 35, No. 151, - pp. 773--782, 1980. - */ - bound = (m <= k) ? m : k; - ++k; - end = (end + 1) % m; - - /* Compute the negative of gradients. */ - vecncpy(d, g, n); - - j = end; - for (i = 0; i < bound; ++i) - { - j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */ - it = &lm[j]; - /* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */ - vecdot(&it->alpha, it->s, d, n); - it->alpha /= it->ys; - /* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */ - vecadd(d, it->y, -it->alpha, n); - } - - vecscale(d, ys / yy, n); - - for (i = 0; i < bound; ++i) - { - it = &lm[j]; - /* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamm_{i}. */ - vecdot(&beta, it->y, d, n); - beta /= it->ys; - /* \gamm_{i+1} = \gamm_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */ - vecadd(d, it->s, it->alpha - beta, n); - j = (j + 1) % m; /* if (++j == m) j = 0; */ - } - - /* - Now the search direction d is ready. We try step = 1 first. - */ - step = 1.0; - } - } - - /* Return the final value of the objective function. */ - if (ptr_fx != NULL) - { - *ptr_fx = fx; - } - - vecfree(pf); - - /* Free memory blocks used by this function. */ - if (lm != NULL) - { - for (i = 0; i < m; ++i) - { - vecfree(lm[i].s); - vecfree(lm[i].y); - } - vecfree(lm); - } - vecfree(d); - vecfree(gp); - vecfree(g); - vecfree(xp); - - return ret; - } - - /** - * Get string description of an lbfgs_optimize() return code. - * - * @param err A value returned by lbfgs_optimize(). - */ - inline const char *lbfgs_strerror(int err) - { - switch (err) - { - case LBFGS_CONVERGENCE: - return "Success: reached convergence (g_epsilon)."; - - case LBFGS_STOP: - return "Success: met stopping criteria (past f decrease less than delta)."; - - case LBFGS_ALREADY_MINIMIZED: - return "The initial variables already minimize the objective function."; - - case LBFGSERR_UNKNOWNERROR: - return "Unknown error."; - - case LBFGSERR_LOGICERROR: - return "Logic error."; - - case LBFGSERR_CANCELED: - return "The minimization process has been canceled."; - - case LBFGSERR_INVALID_N: - return "Invalid number of variables specified."; - - case LBFGSERR_INVALID_MEMSIZE: - return "Invalid parameter lbfgs_parameter_t::mem_size specified."; - - case LBFGSERR_INVALID_GEPSILON: - return "Invalid parameter lbfgs_parameter_t::g_epsilon specified."; - - case LBFGSERR_INVALID_TESTPERIOD: - return "Invalid parameter lbfgs_parameter_t::past specified."; - - case LBFGSERR_INVALID_DELTA: - return "Invalid parameter lbfgs_parameter_t::delta specified."; - - case LBFGSERR_INVALID_MINSTEP: - return "Invalid parameter lbfgs_parameter_t::min_step specified."; - - case LBFGSERR_INVALID_MAXSTEP: - return "Invalid parameter lbfgs_parameter_t::max_step specified."; - - case LBFGSERR_INVALID_FDECCOEFF: - return "Invalid parameter lbfgs_parameter_t::f_dec_coeff specified."; - - case LBFGSERR_INVALID_SCURVCOEFF: - return "Invalid parameter lbfgs_parameter_t::s_curv_coeff specified."; - - case LBFGSERR_INVALID_XTOL: - return "Invalid parameter lbfgs_parameter_t::xtol specified."; - - case LBFGSERR_INVALID_MAXLINESEARCH: - return "Invalid parameter lbfgs_parameter_t::max_linesearch specified."; - - case LBFGSERR_OUTOFINTERVAL: - return "The line-search step went out of the interval of uncertainty."; - - case LBFGSERR_INCORRECT_TMINMAX: - return "A logic error occurred; alternatively, the interval of uncertainty" - " became too small."; - - case LBFGSERR_ROUNDING_ERROR: - return "A rounding error occurred; alternatively, no line-search step" - " satisfies the sufficient decrease and curvature conditions."; - - case LBFGSERR_MINIMUMSTEP: - return "The line-search step became smaller than lbfgs_parameter_t::min_step."; - - case LBFGSERR_MAXIMUMSTEP: - return "The line-search step became larger than lbfgs_parameter_t::max_step."; - - case LBFGSERR_MAXIMUMLINESEARCH: - return "The line-search routine reaches the maximum number of evaluations."; - - case LBFGSERR_MAXIMUMITERATION: - return "The algorithm routine reaches the maximum number of iterations."; - - case LBFGSERR_WIDTHTOOSMALL: - return "Relative width of the interval of uncertainty is at most" - " lbfgs_parameter_t::xtol."; - - case LBFGSERR_INVALIDPARAMETERS: - return "A logic error (negative line-search step) occurred."; - - case LBFGSERR_INCREASEGRADIENT: - return "The current search direction increases the objective function value."; - - default: - return "(unknown)"; - } - } - -} // namespace lbfgs - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/uniform_bspline.h b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/uniform_bspline.h deleted file mode 100644 index d2507a27..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/uniform_bspline.h +++ /dev/null @@ -1,80 +0,0 @@ -#ifndef _UNIFORM_BSPLINE_H_ -#define _UNIFORM_BSPLINE_H_ - -#include -#include -#include - -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 &point_set, - const vector &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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/package.xml b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/package.xml deleted file mode 100644 index 02e098be..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/package.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - bspline_opt - 0.0.0 - The bspline_opt package - - - - - iszhouxin - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - plan_env - path_searching - traj_utils - roscpp - rospy - std_msgs - plan_env - path_searching - traj_utils - roscpp - rospy - std_msgs - plan_env - path_searching - traj_utils - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/bspline_optimizer.cpp b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/bspline_optimizer.cpp deleted file mode 100644 index 4d113323..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/bspline_optimizer.cpp +++ /dev/null @@ -1,1735 +0,0 @@ -#include "bspline_opt/bspline_optimizer.h" -#include "bspline_opt/gradient_descent_optimizer.h" -// using namespace std; - -namespace ego_planner -{ - - void BsplineOptimizer::setParam(ros::NodeHandle &nh) - { - nh.param("optimization/lambda_smooth", lambda1_, -1.0); - nh.param("optimization/lambda_collision", lambda2_, -1.0); - nh.param("optimization/lambda_feasibility", lambda3_, -1.0); - nh.param("optimization/lambda_fitness", lambda4_, -1.0); - - nh.param("optimization/dist0", dist0_, -1.0); - nh.param("optimization/swarm_clearance", swarm_clearance_, -1.0); - nh.param("optimization/max_vel", max_vel_, -1.0); - nh.param("optimization/max_acc", max_acc_, -1.0); - // 3维 - nh.param("optimization/order", order_, 3); - } - - void BsplineOptimizer::setEnvironment(const GridMap::Ptr &map) - { - this->grid_map_ = map; - } - - void BsplineOptimizer::setEnvironment(const GridMap::Ptr &map, const fast_planner::ObjPredictor::Ptr mov_obj) - { - this->grid_map_ = map; - this->moving_objs_ = mov_obj; - } - - void BsplineOptimizer::setControlPoints(const Eigen::MatrixXd &points) - { - cps_.points = points; - } - - void BsplineOptimizer::setBsplineInterval(const double &ts) { bspline_interval_ = ts; } - - void BsplineOptimizer::setSwarmTrajs(SwarmTrajData *swarm_trajs_ptr) { swarm_trajs_ = swarm_trajs_ptr; } - - void BsplineOptimizer::setDroneId(const int drone_id) { drone_id_ = drone_id; } - - std::vector BsplineOptimizer::distinctiveTrajs(vector> segments) - { - if (segments.size() == 0) // will be invoked again later. - { - std::vector oneSeg; - oneSeg.push_back(cps_); - return oneSeg; - } - - constexpr int MAX_TRAJS = 8; - constexpr int VARIS = 2; - int seg_upbound = std::min((int)segments.size(), static_cast(floor(log(MAX_TRAJS) / log(VARIS)))); - std::vector control_pts_buf; - control_pts_buf.reserve(MAX_TRAJS); - const double RESOLUTION = grid_map_->getResolution(); - const double CTRL_PT_DIST = (cps_.points.col(0) - cps_.points.col(cps_.size - 1)).norm() / (cps_.size - 1); - - // Step 1. Find the opposite vectors and base points for every segment. - std::vector> RichInfoSegs; - for (int i = 0; i < seg_upbound; i++) - { - std::pair RichInfoOneSeg; - ControlPoints RichInfoOneSeg_temp; - cps_.segment(RichInfoOneSeg_temp, segments[i].first, segments[i].second); - RichInfoOneSeg.first = RichInfoOneSeg_temp; - RichInfoOneSeg.second = RichInfoOneSeg_temp; - RichInfoSegs.push_back(RichInfoOneSeg); - - // cout << "RichInfoOneSeg_temp, out" << endl; - // cout << "RichInfoSegs[" << i << "].first" << endl; - // for ( int k=0; k 0 ) - // { - // cout << "###" << RichInfoOneSeg_temp.points.col(k).transpose() << endl; - // for (int k2 = 0; k2 < RichInfoOneSeg_temp.base_point[k].size(); k2++) - // { - // cout << " " << RichInfoOneSeg_temp.base_point[k][k2].transpose() << " @ " << RichInfoOneSeg_temp.direction[k][k2].transpose() << endl; - // } - // } - } - - for (int i = 0; i < seg_upbound; i++) - { - - // 1.1 Find the start occupied point id and the last occupied point id - if (RichInfoSegs[i].first.size > 1) - { - int occ_start_id = -1, occ_end_id = -1; - Eigen::Vector3d occ_start_pt, occ_end_pt; - for (int j = 0; j < RichInfoSegs[i].first.size - 1; j++) - { - //cout << "A *" << j << "*" << endl; - double step_size = RESOLUTION / (RichInfoSegs[i].first.points.col(j) - RichInfoSegs[i].first.points.col(j + 1)).norm() / 2; - for (double a = 1; a > 0; a -= step_size) - { - Eigen::Vector3d pt(a * RichInfoSegs[i].first.points.col(j) + (1 - a) * RichInfoSegs[i].first.points.col(j + 1)); - //cout << " " << grid_map_->getInflateOccupancy(pt) << " pt=" << pt.transpose() << endl; - if (grid_map_->getInflateOccupancy(pt)) - { - occ_start_id = j; - occ_start_pt = pt; - goto exit_multi_loop1; - } - } - } - exit_multi_loop1:; - for (int j = RichInfoSegs[i].first.size - 1; j >= 1; j--) - { - //cout << "j=" << j << endl; - //cout << "B *" << j << "*" << endl; - ; - double step_size = RESOLUTION / (RichInfoSegs[i].first.points.col(j) - RichInfoSegs[i].first.points.col(j - 1)).norm(); - for (double a = 1; a > 0; a -= step_size) - { - Eigen::Vector3d pt(a * RichInfoSegs[i].first.points.col(j) + (1 - a) * RichInfoSegs[i].first.points.col(j - 1)); - //cout << " " << grid_map_->getInflateOccupancy(pt) << " pt=" << pt.transpose() << endl; - ; - if (grid_map_->getInflateOccupancy(pt)) - { - occ_end_id = j; - occ_end_pt = pt; - goto exit_multi_loop2; - } - } - } - exit_multi_loop2:; - - // double check - if (occ_start_id == -1 || occ_end_id == -1) - { - // It means that the first or the last control points of one segment are in obstacles, which is not allowed. - // ROS_WARN("What? occ_start_id=%d, occ_end_id=%d", occ_start_id, occ_end_id); - - segments.erase(segments.begin() + i); - RichInfoSegs.erase(RichInfoSegs.begin() + i); - seg_upbound--; - i--; - - continue; - - // cout << "RichInfoSegs[" << i << "].first" << endl; - // for (int k = 0; k < RichInfoSegs[i].first.size; k++) - // { - // if (RichInfoSegs[i].first.base_point.size() > 0) - // { - // cout << "###" << RichInfoSegs[i].first.points.col(k).transpose() << endl; - // for (int k2 = 0; k2 < RichInfoSegs[i].first.base_point[k].size(); k2++) - // { - // cout << " " << RichInfoSegs[i].first.base_point[k][k2].transpose() << " @ " << RichInfoSegs[i].first.direction[k][k2].transpose() << endl; - // } - // } - // } - } - - // 1.2 Reverse the vector and find new base points from occ_start_id to occ_end_id. - for (int j = occ_start_id; j <= occ_end_id; j++) - { - Eigen::Vector3d base_pt_reverse, base_vec_reverse; - if (RichInfoSegs[i].first.base_point[j].size() != 1) - { - cout << "RichInfoSegs[" << i << "].first.base_point[" << j << "].size()=" << RichInfoSegs[i].first.base_point[j].size() << endl; - ROS_ERROR("Wrong number of base_points!!! Should not be happen!."); - - cout << setprecision(5); - cout << "cps_" << endl; - cout << " clearance=" << cps_.clearance << " cps.size=" << cps_.size << endl; - for (int temp_i = 0; temp_i < cps_.size; temp_i++) - { - if (cps_.base_point[temp_i].size() > 1 && cps_.base_point[temp_i].size() < 1000) - { - ROS_ERROR("Should not happen!!!"); - cout << "######" << cps_.points.col(temp_i).transpose() << endl; - for (size_t temp_j = 0; temp_j < cps_.base_point[temp_i].size(); temp_j++) - cout << " " << cps_.base_point[temp_i][temp_j].transpose() << " @ " << cps_.direction[temp_i][temp_j].transpose() << endl; - } - } - - std::vector blank; - return blank; - } - - base_vec_reverse = -RichInfoSegs[i].first.direction[j][0]; - - // The start and the end case must get taken special care of. - if (j == occ_start_id) - { - base_pt_reverse = occ_start_pt; - } - else if (j == occ_end_id) - { - base_pt_reverse = occ_end_pt; - } - else - { - base_pt_reverse = RichInfoSegs[i].first.points.col(j) + base_vec_reverse * (RichInfoSegs[i].first.base_point[j][0] - RichInfoSegs[i].first.points.col(j)).norm(); - } - - if (grid_map_->getInflateOccupancy(base_pt_reverse)) // Search outward. - { - double l_upbound = 5 * CTRL_PT_DIST; // "5" is the threshold. - double l = RESOLUTION; - for (; l <= l_upbound; l += RESOLUTION) - { - Eigen::Vector3d base_pt_temp = base_pt_reverse + l * base_vec_reverse; - //cout << base_pt_temp.transpose() << endl; - if (!grid_map_->getInflateOccupancy(base_pt_temp)) - { - RichInfoSegs[i].second.base_point[j][0] = base_pt_temp; - RichInfoSegs[i].second.direction[j][0] = base_vec_reverse; - break; - } - } - if (l > l_upbound) - { - ROS_WARN("Can't find the new base points at the opposite within the threshold. i=%d, j=%d", i, j); - - segments.erase(segments.begin() + i); - RichInfoSegs.erase(RichInfoSegs.begin() + i); - seg_upbound--; - i--; - - goto exit_multi_loop3; // break "for (int j = 0; j < RichInfoSegs[i].first.size; j++)" - } - } - else if ((base_pt_reverse - RichInfoSegs[i].first.points.col(j)).norm() >= RESOLUTION) // Unnecessary to search. - { - RichInfoSegs[i].second.base_point[j][0] = base_pt_reverse; - RichInfoSegs[i].second.direction[j][0] = base_vec_reverse; - } - else - { - ROS_WARN("base_point and control point are too close!"); - cout << "base_point=" << RichInfoSegs[i].first.base_point[j][0].transpose() << " control point=" << RichInfoSegs[i].first.points.col(j).transpose() << endl; - - segments.erase(segments.begin() + i); - RichInfoSegs.erase(RichInfoSegs.begin() + i); - seg_upbound--; - i--; - - goto exit_multi_loop3; // break "for (int j = 0; j < RichInfoSegs[i].first.size; j++)" - } - } - - // 1.3 Assign the base points to control points within [0, occ_start_id) and (occ_end_id, RichInfoSegs[i].first.size()-1]. - if (RichInfoSegs[i].second.size) - { - for (int j = occ_start_id - 1; j >= 0; j--) - { - RichInfoSegs[i].second.base_point[j][0] = RichInfoSegs[i].second.base_point[occ_start_id][0]; - RichInfoSegs[i].second.direction[j][0] = RichInfoSegs[i].second.direction[occ_start_id][0]; - } - for (int j = occ_end_id + 1; j < RichInfoSegs[i].second.size; j++) - { - RichInfoSegs[i].second.base_point[j][0] = RichInfoSegs[i].second.base_point[occ_end_id][0]; - RichInfoSegs[i].second.direction[j][0] = RichInfoSegs[i].second.direction[occ_end_id][0]; - } - } - - exit_multi_loop3:; - } - else if (RichInfoSegs[i].first.size == 1) - { - cout << "i=" << i << " RichInfoSegs.size()=" << RichInfoSegs.size() << endl; - cout << "RichInfoSegs[i].first.size=" << RichInfoSegs[i].first.size << endl; - cout << "RichInfoSegs[i].first.direction.size()=" << RichInfoSegs[i].first.direction.size() << endl; - cout << "RichInfoSegs[i].first.direction[0].size()=" << RichInfoSegs[i].first.direction[0].size() << endl; - cout << "RichInfoSegs[i].first.points.cols()=" << RichInfoSegs[i].first.points.cols() << endl; - cout << "RichInfoSegs[i].first.base_point.size()=" << RichInfoSegs[i].first.base_point.size() << endl; - cout << "RichInfoSegs[i].first.base_point[0].size()=" << RichInfoSegs[i].first.base_point[0].size() << endl; - Eigen::Vector3d base_vec_reverse = -RichInfoSegs[i].first.direction[0][0]; - Eigen::Vector3d base_pt_reverse = RichInfoSegs[i].first.points.col(0) + base_vec_reverse * (RichInfoSegs[i].first.base_point[0][0] - RichInfoSegs[i].first.points.col(0)).norm(); - - if (grid_map_->getInflateOccupancy(base_pt_reverse)) // Search outward. - { - double l_upbound = 5 * CTRL_PT_DIST; // "5" is the threshold. - double l = RESOLUTION; - for (; l <= l_upbound; l += RESOLUTION) - { - Eigen::Vector3d base_pt_temp = base_pt_reverse + l * base_vec_reverse; - //cout << base_pt_temp.transpose() << endl; - if (!grid_map_->getInflateOccupancy(base_pt_temp)) - { - RichInfoSegs[i].second.base_point[0][0] = base_pt_temp; - RichInfoSegs[i].second.direction[0][0] = base_vec_reverse; - break; - } - } - if (l > l_upbound) - { - ROS_WARN("Can't find the new base points at the opposite within the threshold, 2. i=%d", i); - - segments.erase(segments.begin() + i); - RichInfoSegs.erase(RichInfoSegs.begin() + i); - seg_upbound--; - i--; - } - } - else if ((base_pt_reverse - RichInfoSegs[i].first.points.col(0)).norm() >= RESOLUTION) // Unnecessary to search. - { - RichInfoSegs[i].second.base_point[0][0] = base_pt_reverse; - RichInfoSegs[i].second.direction[0][0] = base_vec_reverse; - } - else - { - ROS_WARN("base_point and control point are too close!, 2"); - cout << "base_point=" << RichInfoSegs[i].first.base_point[0][0].transpose() << " control point=" << RichInfoSegs[i].first.points.col(0).transpose() << endl; - - segments.erase(segments.begin() + i); - RichInfoSegs.erase(RichInfoSegs.begin() + i); - seg_upbound--; - i--; - } - } - else - { - segments.erase(segments.begin() + i); - RichInfoSegs.erase(RichInfoSegs.begin() + i); - seg_upbound--; - i--; - } - } - // cout << "A3" << endl; - - // Step 2. Assemble each segment to make up the new control point sequence. - if (seg_upbound == 0) // After the erase operation above, segment legth will decrease to 0 again. - { - std::vector oneSeg; - oneSeg.push_back(cps_); - return oneSeg; - } - - std::vector selection(seg_upbound); - std::fill(selection.begin(), selection.end(), 0); - selection[0] = -1; // init - int max_traj_nums = static_cast(pow(VARIS, seg_upbound)); - for (int i = 0; i < max_traj_nums; i++) - { - // 2.1 Calculate the selection table. - int digit_id = 0; - selection[digit_id]++; - while (digit_id < seg_upbound && selection[digit_id] >= VARIS) - { - selection[digit_id] = 0; - digit_id++; - if (digit_id >= seg_upbound) - { - ROS_ERROR("Should not happen!!! digit_id=%d, seg_upbound=%d", digit_id, seg_upbound); - } - selection[digit_id]++; - } - - // 2.2 Assign params according to the selection table. - ControlPoints cpsOneSample; - cpsOneSample.resize(cps_.size); - cpsOneSample.clearance = cps_.clearance; - int cp_id = 0, seg_id = 0, cp_of_seg_id = 0; - while (/*seg_id < RichInfoSegs.size() ||*/ cp_id < cps_.size) - { - //cout << "A "; - // if ( seg_id >= RichInfoSegs.size() ) - // { - // cout << "seg_id=" << seg_id << " RichInfoSegs.size()=" << RichInfoSegs.size() << endl; - // } - // if ( cp_id >= cps_.base_point.size() ) - // { - // cout << "cp_id=" << cp_id << " cps_.base_point.size()=" << cps_.base_point.size() << endl; - // } - // if ( cp_of_seg_id >= RichInfoSegs[seg_id].first.base_point.size() ) - // { - // cout << "cp_of_seg_id=" << cp_of_seg_id << " RichInfoSegs[seg_id].first.base_point.size()=" << RichInfoSegs[seg_id].first.base_point.size() << endl; - // } - - if (seg_id >= seg_upbound || cp_id < segments[seg_id].first || cp_id > segments[seg_id].second) - { - cpsOneSample.points.col(cp_id) = cps_.points.col(cp_id); - cpsOneSample.base_point[cp_id] = cps_.base_point[cp_id]; - cpsOneSample.direction[cp_id] = cps_.direction[cp_id]; - } - else if (cp_id >= segments[seg_id].first && cp_id <= segments[seg_id].second) - { - if (!selection[seg_id]) // zx-todo - { - cpsOneSample.points.col(cp_id) = RichInfoSegs[seg_id].first.points.col(cp_of_seg_id); - cpsOneSample.base_point[cp_id] = RichInfoSegs[seg_id].first.base_point[cp_of_seg_id]; - cpsOneSample.direction[cp_id] = RichInfoSegs[seg_id].first.direction[cp_of_seg_id]; - cp_of_seg_id++; - } - else - { - if (RichInfoSegs[seg_id].second.size) - { - cpsOneSample.points.col(cp_id) = RichInfoSegs[seg_id].second.points.col(cp_of_seg_id); - cpsOneSample.base_point[cp_id] = RichInfoSegs[seg_id].second.base_point[cp_of_seg_id]; - cpsOneSample.direction[cp_id] = RichInfoSegs[seg_id].second.direction[cp_of_seg_id]; - cp_of_seg_id++; - } - else - { - // Abandon this trajectory. - goto abandon_this_trajectory; - } - } - - if (cp_id == segments[seg_id].second) - { - cp_of_seg_id = 0; - seg_id++; - } - } - else - { - ROS_ERROR("Shold not happen!!!!, cp_id=%d, seg_id=%d, segments.front().first=%d, segments.back().second=%d, segments[seg_id].first=%d, segments[seg_id].second=%d", - cp_id, seg_id, segments.front().first, segments.back().second, segments[seg_id].first, segments[seg_id].second); - } - - cp_id++; - } - - control_pts_buf.push_back(cpsOneSample); - - abandon_this_trajectory:; - } - - return control_pts_buf; - } // namespace ego_planner - - /* This function is very similar to check_collision_and_rebound(). - * It was written separately, just because I did it once and it has been running stably since March 2020. - * But I will merge then someday.*/ - std::vector> BsplineOptimizer::initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init /*= true*/) - { - - if (flag_first_init) - { - cps_.clearance = dist0_; - cps_.resize(init_points.cols()); - cps_.points = init_points; - } - - /*** Segment the initial trajectory according to obstacles ***/ - constexpr int ENOUGH_INTERVAL = 2; - double step_size = grid_map_->getResolution() / ((init_points.col(0) - init_points.rightCols(1)).norm() / (init_points.cols() - 1)) / 1.5; - int in_id = -1, out_id = -1; - vector> segment_ids; - int same_occ_state_times = ENOUGH_INTERVAL + 1; - bool occ, last_occ = false; - bool flag_got_start = false, flag_got_end = false, flag_got_end_maybe = false; - int i_end = (int)init_points.cols() - order_ - ((int)init_points.cols() - 2 * order_) / 3; // only check closed 2/3 points. - for (int i = order_; i <= i_end; ++i) - { - //cout << " *" << i-1 << "*" ; - for (double a = 1.0; a > 0.0; a -= step_size) - { - occ = grid_map_->getInflateOccupancy(a * init_points.col(i - 1) + (1 - a) * init_points.col(i)); - //cout << " " << occ; - // cout << setprecision(5); - // cout << (a * init_points.col(i-1) + (1-a) * init_points.col(i)).transpose() << " occ1=" << occ << endl; - - if (occ && !last_occ) - { - if (same_occ_state_times > ENOUGH_INTERVAL || i == order_) - { - in_id = i - 1; - flag_got_start = true; - } - same_occ_state_times = 0; - flag_got_end_maybe = false; // terminate in advance - } - else if (!occ && last_occ) - { - out_id = i; - flag_got_end_maybe = true; - same_occ_state_times = 0; - } - else - { - ++same_occ_state_times; - } - - if (flag_got_end_maybe && (same_occ_state_times > ENOUGH_INTERVAL || (i == (int)init_points.cols() - order_))) - { - flag_got_end_maybe = false; - flag_got_end = true; - } - - last_occ = occ; - - if (flag_got_start && flag_got_end) - { - flag_got_start = false; - flag_got_end = false; - segment_ids.push_back(std::pair(in_id, out_id)); - } - } - } - // cout << endl; - - // for (size_t i = 0; i < segment_ids.size(); i++) - // { - // cout << "segment_ids=" << segment_ids[i].first << " ~ " << segment_ids[i].second << endl; - // } - - // return in advance - if (segment_ids.size() == 0) - { - vector> blank_ret; - return blank_ret; - } - - /*** a star search ***/ - vector> a_star_pathes; - for (size_t i = 0; i < segment_ids.size(); ++i) - { - //cout << "in=" << in.transpose() << " out=" << out.transpose() << endl; - Eigen::Vector3d in(init_points.col(segment_ids[i].first)), out(init_points.col(segment_ids[i].second)); - if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out)) - { - a_star_pathes.push_back(a_star_->getPath()); - } - else - { - ROS_ERROR("a star error, force return!"); - vector> blank_ret; - return blank_ret; - } - } - - /*** calculate bounds ***/ - int id_low_bound, id_up_bound; - vector> bounds(segment_ids.size()); - for (size_t i = 0; i < segment_ids.size(); i++) - { - - if (i == 0) // first segment - { - id_low_bound = order_; - if (segment_ids.size() > 1) - { - id_up_bound = (int)(((segment_ids[0].second + segment_ids[1].first) - 1.0f) / 2); // id_up_bound : -1.0f fix() - } - else - { - id_up_bound = init_points.cols() - order_ - 1; - } - } - else if (i == segment_ids.size() - 1) // last segment, i != 0 here - { - id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil() - id_up_bound = init_points.cols() - order_ - 1; - } - else - { - id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil() - id_up_bound = (int)(((segment_ids[i].second + segment_ids[i + 1].first) - 1.0f) / 2); // id_up_bound : -1.0f fix() - } - - bounds[i] = std::pair(id_low_bound, id_up_bound); - } - - // cout << "+++++++++" << endl; - // for ( int j=0; j> adjusted_segment_ids(segment_ids.size()); - constexpr double MINIMUM_PERCENT = 0.0; // Each segment is guaranteed to have sufficient points to generate sufficient force - int minimum_points = round(init_points.cols() * MINIMUM_PERCENT), num_points; - for (size_t i = 0; i < segment_ids.size(); i++) - { - /*** Adjust segment length ***/ - num_points = segment_ids[i].second - segment_ids[i].first + 1; - //cout << "i = " << i << " first = " << segment_ids[i].first << " second = " << segment_ids[i].second << endl; - if (num_points < minimum_points) - { - double add_points_each_side = (int)(((minimum_points - num_points) + 1.0f) / 2); - - adjusted_segment_ids[i].first = segment_ids[i].first - add_points_each_side >= bounds[i].first ? segment_ids[i].first - add_points_each_side : bounds[i].first; - - adjusted_segment_ids[i].second = segment_ids[i].second + add_points_each_side <= bounds[i].second ? segment_ids[i].second + add_points_each_side : bounds[i].second; - } - else - { - adjusted_segment_ids[i].first = segment_ids[i].first; - adjusted_segment_ids[i].second = segment_ids[i].second; - } - - //cout << "final:" << "i = " << i << " first = " << adjusted_segment_ids[i].first << " second = " << adjusted_segment_ids[i].second << endl; - } - for (size_t i = 1; i < adjusted_segment_ids.size(); i++) // Avoid overlap - { - if (adjusted_segment_ids[i - 1].second >= adjusted_segment_ids[i].first) - { - double middle = (double)(adjusted_segment_ids[i - 1].second + adjusted_segment_ids[i].first) / 2.0; - adjusted_segment_ids[i - 1].second = static_cast(middle - 0.1); - adjusted_segment_ids[i].first = static_cast(middle + 1.1); - } - } - - // Used for return - vector> final_segment_ids; - - /*** Assign data to each segment ***/ - for (size_t i = 0; i < segment_ids.size(); i++) - { - // step 1 - for (int j = adjusted_segment_ids[i].first; j <= adjusted_segment_ids[i].second; ++j) - cps_.flag_temp[j] = false; - - // step 2 - int got_intersection_id = -1; - for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j) - { - Eigen::Vector3d ctrl_pts_law(init_points.col(j + 1) - init_points.col(j - 1)), intersection_point; - int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation - double val = (a_star_pathes[i][Astar_id] - init_points.col(j)).dot(ctrl_pts_law), last_val = val; - while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) - { - last_Astar_id = Astar_id; - - if (val >= 0) - --Astar_id; - else - ++Astar_id; - - val = (a_star_pathes[i][Astar_id] - init_points.col(j)).dot(ctrl_pts_law); - - if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed - { - intersection_point = - a_star_pathes[i][Astar_id] + - ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * - (ctrl_pts_law.dot(init_points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t - ); - - //cout << "i=" << i << " j=" << j << " Astar_id=" << Astar_id << " last_Astar_id=" << last_Astar_id << " intersection_point = " << intersection_point.transpose() << endl; - - got_intersection_id = j; - break; - } - } - - if (got_intersection_id >= 0) - { - double length = (intersection_point - init_points.col(j)).norm(); - if (length > 1e-5) - { - cps_.flag_temp[j] = true; - for (double a = length; a >= 0.0; a -= grid_map_->getResolution()) - { - occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * init_points.col(j)); - - if (occ || a < grid_map_->getResolution()) - { - if (occ) - a += grid_map_->getResolution(); - cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * init_points.col(j)); - cps_.direction[j].push_back((intersection_point - init_points.col(j)).normalized()); - // cout << "A " << j << endl; - break; - } - } - } - else - { - got_intersection_id = -1; - } - } - } - - /* Corner case: the segment length is too short. Here the control points may outside the A* path, leading to opposite gradient direction. So I have to take special care of it */ - if (segment_ids[i].second - segment_ids[i].first == 1) - { - Eigen::Vector3d ctrl_pts_law(init_points.col(segment_ids[i].second) - init_points.col(segment_ids[i].first)), intersection_point; - Eigen::Vector3d middle_point = (init_points.col(segment_ids[i].second) + init_points.col(segment_ids[i].first)) / 2; - int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation - double val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law), last_val = val; - while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) - { - last_Astar_id = Astar_id; - - if (val >= 0) - --Astar_id; - else - ++Astar_id; - - val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law); - - if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed - { - intersection_point = - a_star_pathes[i][Astar_id] + - ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * - (ctrl_pts_law.dot(middle_point - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t - ); - - if ((intersection_point - middle_point).norm() > 0.01) // 1cm. - { - cps_.flag_temp[segment_ids[i].first] = true; - cps_.base_point[segment_ids[i].first].push_back(init_points.col(segment_ids[i].first)); - cps_.direction[segment_ids[i].first].push_back((intersection_point - middle_point).normalized()); - - got_intersection_id = segment_ids[i].first; - } - break; - } - } - } - - //step 3 - if (got_intersection_id >= 0) - { - for (int j = got_intersection_id + 1; j <= adjusted_segment_ids[i].second; ++j) - if (!cps_.flag_temp[j]) - { - cps_.base_point[j].push_back(cps_.base_point[j - 1].back()); - cps_.direction[j].push_back(cps_.direction[j - 1].back()); - // cout << "AAA " << j << endl; - } - - for (int j = got_intersection_id - 1; j >= adjusted_segment_ids[i].first; --j) - if (!cps_.flag_temp[j]) - { - cps_.base_point[j].push_back(cps_.base_point[j + 1].back()); - cps_.direction[j].push_back(cps_.direction[j + 1].back()); - // cout << "AAAA " << j << endl; - } - - final_segment_ids.push_back(adjusted_segment_ids[i]); - } - else - { - // Just ignore, it does not matter ^_^. - // ROS_ERROR("Failed to generate direction! segment_id=%d", i); - } - } - - return final_segment_ids; - } - - int BsplineOptimizer::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) - { - BsplineOptimizer *opt = reinterpret_cast(func_data); - // cout << "k=" << k << endl; - // cout << "opt->flag_continue_to_optimize_=" << opt->flag_continue_to_optimize_ << endl; - return (opt->force_stop_type_ == STOP_FOR_ERROR || opt->force_stop_type_ == STOP_FOR_REBOUND); - } - - double BsplineOptimizer::costFunctionRebound(void *func_data, const double *x, double *grad, const int n) - { - BsplineOptimizer *opt = reinterpret_cast(func_data); - - double cost; - opt->combineCostRebound(x, grad, cost, n); - - opt->iter_num_ += 1; - return cost; - } - - double BsplineOptimizer::costFunctionRefine(void *func_data, const double *x, double *grad, const int n) - { - BsplineOptimizer *opt = reinterpret_cast(func_data); - - double cost; - opt->combineCostRefine(x, grad, cost, n); - - opt->iter_num_ += 1; - return cost; - } - - void BsplineOptimizer::calcSwarmCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) - { - cost = 0.0; - int end_idx = q.cols() - order_ - (double)(q.cols() - 2 * order_) * 1.0 / 3.0; // Only check the first 2/3 points - const double CLEARANCE = swarm_clearance_ * 2; - double t_now = ros::Time::now().toSec(); - constexpr double a = 2.0, b = 1.0, inv_a2 = 1 / a / a, inv_b2 = 1 / b / b; - - for (int i = order_; i < end_idx; i++) - { - double glb_time = t_now + ((double)(order_ - 1) / 2 + (i - order_ + 1)) * bspline_interval_; - - for (size_t id = 0; id < swarm_trajs_->size(); id++) - { - if ((swarm_trajs_->at(id).drone_id != (int)id) || swarm_trajs_->at(id).drone_id == drone_id_) - { - continue; - } - - double traj_i_satrt_time = swarm_trajs_->at(id).start_time_.toSec(); - if (glb_time < traj_i_satrt_time + swarm_trajs_->at(id).duration_ - 0.1) - { - /* def cost=(c-sqrt([Q-O]'D[Q-O]))^2, D=[1/b^2,0,0;0,1/b^2,0;0,0,1/a^2] */ - Eigen::Vector3d swarm_prid = swarm_trajs_->at(id).position_traj_.evaluateDeBoorT(glb_time - traj_i_satrt_time); - Eigen::Vector3d dist_vec = cps_.points.col(i) - swarm_prid; - double ellip_dist = sqrt(dist_vec(2) * dist_vec(2) * inv_a2 + (dist_vec(0) * dist_vec(0) + dist_vec(1) * dist_vec(1)) * inv_b2); - double dist_err = CLEARANCE - ellip_dist; - - Eigen::Vector3d dist_grad = cps_.points.col(i) - swarm_prid; - Eigen::Vector3d Coeff; - Coeff(0) = -2 * (CLEARANCE / ellip_dist - 1) * inv_b2; - Coeff(1) = Coeff(0); - Coeff(2) = -2 * (CLEARANCE / ellip_dist - 1) * inv_a2; - - if (dist_err < 0) - { - /* do nothing */ - } - else - { - cost += pow(dist_err, 2); - gradient.col(i) += (Coeff.array() * dist_grad.array()).matrix(); - } - - if (min_ellip_dist_ > dist_err) - { - min_ellip_dist_ = dist_err; - } - } - } - } - } - - void BsplineOptimizer::calcMovingObjCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) - { - cost = 0.0; - int end_idx = q.cols() - order_; - constexpr double CLEARANCE = 1.5; - double t_now = ros::Time::now().toSec(); - - for (int i = order_; i < end_idx; i++) - { - double time = ((double)(order_ - 1) / 2 + (i - order_ + 1)) * bspline_interval_; - - for (int id = 0; id < moving_objs_->getObjNums(); id++) - { - Eigen::Vector3d obj_prid = moving_objs_->evaluateConstVel(id, t_now + time); - double dist = (cps_.points.col(i) - obj_prid).norm(); - //cout /*<< "cps_.points.col(i)=" << cps_.points.col(i).transpose()*/ << " moving_objs_=" << obj_prid.transpose() << " dist=" << dist << endl; - double dist_err = CLEARANCE - dist; - Eigen::Vector3d dist_grad = (cps_.points.col(i) - obj_prid).normalized(); - - if (dist_err < 0) - { - /* do nothing */ - } - else - { - cost += pow(dist_err, 2); - gradient.col(i) += -2.0 * dist_err * dist_grad; - } - } - // cout << "time=" << time << " i=" << i << " order_=" << order_ << " end_idx=" << end_idx << endl; - // cout << "--" << endl; - } - // cout << "---------------" << endl; - } - - void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost, - Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost) - { - cost = 0.0; - int end_idx = q.cols() - order_; - double demarcation = cps_.clearance; - double a = 3 * demarcation, b = -3 * pow(demarcation, 2), c = pow(demarcation, 3); - - force_stop_type_ = DONT_STOP; - if (iter_num > 3 && smoothness_cost / (cps_.size - 2 * order_) < 0.1) // 0.1 is an experimental value that indicates the trajectory is smooth enough. - { - check_collision_and_rebound(); - } - - /*** calculate distance cost and gradient ***/ - for (auto i = order_; i < end_idx; ++i) - { - for (size_t j = 0; j < cps_.direction[i].size(); ++j) - { - double dist = (cps_.points.col(i) - cps_.base_point[i][j]).dot(cps_.direction[i][j]); - double dist_err = cps_.clearance - dist; - Eigen::Vector3d dist_grad = cps_.direction[i][j]; - - if (dist_err < 0) - { - /* do nothing */ - } - else if (dist_err < demarcation) - { - cost += pow(dist_err, 3); - gradient.col(i) += -3.0 * dist_err * dist_err * dist_grad; - } - else - { - cost += a * dist_err * dist_err + b * dist_err + c; - gradient.col(i) += -(2.0 * a * dist_err + b) * dist_grad; - } - } - } - } - - void BsplineOptimizer::calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) - { - - cost = 0.0; - - int end_idx = q.cols() - order_; - - // def: f = |x*v|^2/a^2 + |x×v|^2/b^2 - double a2 = 25, b2 = 1; - for (auto i = order_ - 1; i < end_idx + 1; ++i) - { - Eigen::Vector3d x = (q.col(i - 1) + 4 * q.col(i) + q.col(i + 1)) / 6.0 - ref_pts_[i - 1]; - Eigen::Vector3d v = (ref_pts_[i] - ref_pts_[i - 2]).normalized(); - - double xdotv = x.dot(v); - Eigen::Vector3d xcrossv = x.cross(v); - - double f = pow((xdotv), 2) / a2 + pow(xcrossv.norm(), 2) / b2; - cost += f; - - Eigen::Matrix3d m; - m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; - Eigen::Vector3d df_dx = 2 * xdotv / a2 * v + 2 / b2 * m * xcrossv; - - gradient.col(i - 1) += df_dx / 6; - gradient.col(i) += 4 * df_dx / 6; - gradient.col(i + 1) += df_dx / 6; - } - } - - void BsplineOptimizer::calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost, - Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/) - { - - cost = 0.0; - - if (falg_use_jerk) - { - Eigen::Vector3d jerk, temp_j; - - for (int i = 0; i < q.cols() - 3; i++) - { - /* evaluate jerk */ - jerk = q.col(i + 3) - 3 * q.col(i + 2) + 3 * q.col(i + 1) - q.col(i); - cost += jerk.squaredNorm(); - temp_j = 2.0 * jerk; - /* jerk gradient */ - gradient.col(i + 0) += -temp_j; - gradient.col(i + 1) += 3.0 * temp_j; - gradient.col(i + 2) += -3.0 * temp_j; - gradient.col(i + 3) += temp_j; - } - } - else - { - Eigen::Vector3d acc, temp_acc; - - for (int i = 0; i < q.cols() - 2; i++) - { - /* evaluate acc */ - acc = q.col(i + 2) - 2 * q.col(i + 1) + q.col(i); - cost += acc.squaredNorm(); - temp_acc = 2.0 * acc; - /* acc gradient */ - gradient.col(i + 0) += temp_acc; - gradient.col(i + 1) += -2.0 * temp_acc; - gradient.col(i + 2) += temp_acc; - } - } - } - - void BsplineOptimizer::calcTerminalCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) - { - cost = 0.0; - - // zero cost and gradient in hard constraints - Eigen::Vector3d q_3, q_2, q_1, dq; - q_3 = q.col(q.cols() - 3); - q_2 = q.col(q.cols() - 2); - q_1 = q.col(q.cols() - 1); - - dq = 1 / 6.0 * (q_3 + 4 * q_2 + q_1) - local_target_pt_; - cost += dq.squaredNorm(); - - gradient.col(q.cols() - 3) += 2 * dq * (1 / 6.0); - gradient.col(q.cols() - 2) += 2 * dq * (4 / 6.0); - gradient.col(q.cols() - 1) += 2 * dq * (1 / 6.0); - } - - void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost, - Eigen::MatrixXd &gradient) - { - - //#define SECOND_DERIVATIVE_CONTINOUS - -#ifdef SECOND_DERIVATIVE_CONTINOUS - - cost = 0.0; - double demarcation = 1.0; // 1m/s, 1m/s/s - double ar = 3 * demarcation, br = -3 * pow(demarcation, 2), cr = pow(demarcation, 3); - double al = ar, bl = -br, cl = cr; - - /* abbreviation */ - double ts, ts_inv2, ts_inv3; - ts = bspline_interval_; - ts_inv2 = 1 / ts / ts; - ts_inv3 = 1 / ts / ts / ts; - - /* velocity feasibility */ - for (int i = 0; i < q.cols() - 1; i++) - { - Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts; - - for (int j = 0; j < 3; j++) - { - if (vi(j) > max_vel_ + demarcation) - { - double diff = vi(j) - max_vel_; - cost += (ar * diff * diff + br * diff + cr) * ts_inv3; // multiply ts_inv3 to make vel and acc has similar magnitude - - double grad = (2.0 * ar * diff + br) / ts * ts_inv3; - gradient(j, i + 0) += -grad; - gradient(j, i + 1) += grad; - } - else if (vi(j) > max_vel_) - { - double diff = vi(j) - max_vel_; - cost += pow(diff, 3) * ts_inv3; - ; - - double grad = 3 * diff * diff / ts * ts_inv3; - ; - gradient(j, i + 0) += -grad; - gradient(j, i + 1) += grad; - } - else if (vi(j) < -(max_vel_ + demarcation)) - { - double diff = vi(j) + max_vel_; - cost += (al * diff * diff + bl * diff + cl) * ts_inv3; - - double grad = (2.0 * al * diff + bl) / ts * ts_inv3; - gradient(j, i + 0) += -grad; - gradient(j, i + 1) += grad; - } - else if (vi(j) < -max_vel_) - { - double diff = vi(j) + max_vel_; - cost += -pow(diff, 3) * ts_inv3; - - double grad = -3 * diff * diff / ts * ts_inv3; - gradient(j, i + 0) += -grad; - gradient(j, i + 1) += grad; - } - else - { - /* nothing happened */ - } - } - } - - /* acceleration feasibility */ - for (int i = 0; i < q.cols() - 2; i++) - { - Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2; - - for (int j = 0; j < 3; j++) - { - if (ai(j) > max_acc_ + demarcation) - { - double diff = ai(j) - max_acc_; - cost += ar * diff * diff + br * diff + cr; - - double grad = (2.0 * ar * diff + br) * ts_inv2; - gradient(j, i + 0) += grad; - gradient(j, i + 1) += -2 * grad; - gradient(j, i + 2) += grad; - } - else if (ai(j) > max_acc_) - { - double diff = ai(j) - max_acc_; - cost += pow(diff, 3); - - double grad = 3 * diff * diff * ts_inv2; - gradient(j, i + 0) += grad; - gradient(j, i + 1) += -2 * grad; - gradient(j, i + 2) += grad; - } - else if (ai(j) < -(max_acc_ + demarcation)) - { - double diff = ai(j) + max_acc_; - cost += al * diff * diff + bl * diff + cl; - - double grad = (2.0 * al * diff + bl) * ts_inv2; - gradient(j, i + 0) += grad; - gradient(j, i + 1) += -2 * grad; - gradient(j, i + 2) += grad; - } - else if (ai(j) < -max_acc_) - { - double diff = ai(j) + max_acc_; - cost += -pow(diff, 3); - - double grad = -3 * diff * diff * ts_inv2; - gradient(j, i + 0) += grad; - gradient(j, i + 1) += -2 * grad; - gradient(j, i + 2) += grad; - } - else - { - /* nothing happened */ - } - } - } - -#else - - cost = 0.0; - /* abbreviation */ - double ts, /*vm2, am2, */ ts_inv2; - // vm2 = max_vel_ * max_vel_; - // am2 = max_acc_ * max_acc_; - - ts = bspline_interval_; - ts_inv2 = 1 / ts / ts; - - /* velocity feasibility */ - for (int i = 0; i < q.cols() - 1; i++) - { - Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts; - - //cout << "temp_v * vi=" ; - for (int j = 0; j < 3; j++) - { - if (vi(j) > max_vel_) - { - // cout << "zx-todo VEL" << endl; - // cout << vi(j) << endl; - cost += pow(vi(j) - max_vel_, 2) * ts_inv2; // multiply ts_inv3 to make vel and acc has similar magnitude - - gradient(j, i + 0) += -2 * (vi(j) - max_vel_) / ts * ts_inv2; - gradient(j, i + 1) += 2 * (vi(j) - max_vel_) / ts * ts_inv2; - } - else if (vi(j) < -max_vel_) - { - cost += pow(vi(j) + max_vel_, 2) * ts_inv2; - - gradient(j, i + 0) += -2 * (vi(j) + max_vel_) / ts * ts_inv2; - gradient(j, i + 1) += 2 * (vi(j) + max_vel_) / ts * ts_inv2; - } - else - { - /* code */ - } - } - } - - /* acceleration feasibility */ - for (int i = 0; i < q.cols() - 2; i++) - { - Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2; - - //cout << "temp_a * ai=" ; - for (int j = 0; j < 3; j++) - { - if (ai(j) > max_acc_) - { - // cout << "zx-todo ACC" << endl; - // cout << ai(j) << endl; - cost += pow(ai(j) - max_acc_, 2); - - gradient(j, i + 0) += 2 * (ai(j) - max_acc_) * ts_inv2; - gradient(j, i + 1) += -4 * (ai(j) - max_acc_) * ts_inv2; - gradient(j, i + 2) += 2 * (ai(j) - max_acc_) * ts_inv2; - } - else if (ai(j) < -max_acc_) - { - cost += pow(ai(j) + max_acc_, 2); - - gradient(j, i + 0) += 2 * (ai(j) + max_acc_) * ts_inv2; - gradient(j, i + 1) += -4 * (ai(j) + max_acc_) * ts_inv2; - gradient(j, i + 2) += 2 * (ai(j) + max_acc_) * ts_inv2; - } - else - { - /* code */ - } - } - //cout << endl; - } - -#endif - } - - bool BsplineOptimizer::check_collision_and_rebound(void) - { - - int end_idx = cps_.size - order_; - - /*** Check and segment the initial trajectory according to obstacles ***/ - int in_id, out_id; - vector> segment_ids; - bool flag_new_obs_valid = false; - int i_end = end_idx - (end_idx - order_) / 3; - for (int i = order_ - 1; i <= i_end; ++i) - { - - bool occ = grid_map_->getInflateOccupancy(cps_.points.col(i)); - - /*** check if the new collision will be valid ***/ - if (occ) - { - for (size_t k = 0; k < cps_.direction[i].size(); ++k) - { - cout.precision(2); - if ((cps_.points.col(i) - cps_.base_point[i][k]).dot(cps_.direction[i][k]) < 1 * grid_map_->getResolution()) // current point is outside all the collision_points. - { - occ = false; // Not really takes effect, just for better hunman understanding. - break; - } - } - } - - if (occ) - { - flag_new_obs_valid = true; - - int j; - for (j = i - 1; j >= 0; --j) - { - occ = grid_map_->getInflateOccupancy(cps_.points.col(j)); - if (!occ) - { - in_id = j; - break; - } - } - if (j < 0) // fail to get the obs free point - { - ROS_ERROR("ERROR! the drone is in obstacle. This should not happen."); - in_id = 0; - } - - for (j = i + 1; j < cps_.size; ++j) - { - occ = grid_map_->getInflateOccupancy(cps_.points.col(j)); - - if (!occ) - { - out_id = j; - break; - } - } - if (j >= cps_.size) // fail to get the obs free point - { - ROS_WARN("WARN! terminal point of the current trajectory is in obstacle, skip this planning."); - - force_stop_type_ = STOP_FOR_ERROR; - return false; - } - - i = j + 1; - - segment_ids.push_back(std::pair(in_id, out_id)); - } - } - - if (flag_new_obs_valid) - { - vector> a_star_pathes; - for (size_t i = 0; i < segment_ids.size(); ++i) - { - /*** a star search ***/ - Eigen::Vector3d in(cps_.points.col(segment_ids[i].first)), out(cps_.points.col(segment_ids[i].second)); - if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out)) - { - a_star_pathes.push_back(a_star_->getPath()); - } - else - { - ROS_ERROR("a star error"); - segment_ids.erase(segment_ids.begin() + i); - i--; - } - } - - for (size_t i = 1; i < segment_ids.size(); i++) // Avoid overlap - { - if (segment_ids[i - 1].second >= segment_ids[i].first) - { - double middle = (double)(segment_ids[i - 1].second + segment_ids[i].first) / 2.0; - segment_ids[i - 1].second = static_cast(middle - 0.1); - segment_ids[i].first = static_cast(middle + 1.1); - } - } - - /*** Assign parameters to each segment ***/ - for (size_t i = 0; i < segment_ids.size(); ++i) - { - // step 1 - for (int j = segment_ids[i].first; j <= segment_ids[i].second; ++j) - cps_.flag_temp[j] = false; - - // step 2 - int got_intersection_id = -1; - for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j) - { - Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point; - int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation - double val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val; - while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) - { - last_Astar_id = Astar_id; - - if (val >= 0) - --Astar_id; - else - ++Astar_id; - - val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law); - - // cout << val << endl; - - if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed - { - intersection_point = - a_star_pathes[i][Astar_id] + - ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * - (ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t - ); - - got_intersection_id = j; - break; - } - } - - if (got_intersection_id >= 0) - { - double length = (intersection_point - cps_.points.col(j)).norm(); - if (length > 1e-5) - { - cps_.flag_temp[j] = true; - for (double a = length; a >= 0.0; a -= grid_map_->getResolution()) - { - bool occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j)); - - if (occ || a < grid_map_->getResolution()) - { - if (occ) - a += grid_map_->getResolution(); - cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j)); - cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized()); - break; - } - } - } - else - { - got_intersection_id = -1; - } - } - } - - //step 3 - if (got_intersection_id >= 0) - { - for (int j = got_intersection_id + 1; j <= segment_ids[i].second; ++j) - if (!cps_.flag_temp[j]) - { - cps_.base_point[j].push_back(cps_.base_point[j - 1].back()); - cps_.direction[j].push_back(cps_.direction[j - 1].back()); - } - - for (int j = got_intersection_id - 1; j >= segment_ids[i].first; --j) - if (!cps_.flag_temp[j]) - { - cps_.base_point[j].push_back(cps_.base_point[j + 1].back()); - cps_.direction[j].push_back(cps_.direction[j + 1].back()); - } - } - else - ROS_WARN("Failed to generate direction. It doesn't matter."); - } - - force_stop_type_ = STOP_FOR_REBOUND; - return true; - } - - return false; - } - - bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts) - { - setBsplineInterval(ts); - - double final_cost; - bool flag_success = rebound_optimize(final_cost); - - optimal_points = cps_.points; - - return flag_success; - } - - bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double &final_cost, const ControlPoints &control_points, double ts) - { - setBsplineInterval(ts); - - cps_ = control_points; - - bool flag_success = rebound_optimize(final_cost); - - optimal_points = cps_.points; - - return flag_success; - } - - bool BsplineOptimizer::BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points) - { - - setControlPoints(init_points); - setBsplineInterval(ts); - - bool flag_success = refine_optimize(); - - optimal_points = cps_.points; - - return flag_success; - } - - bool BsplineOptimizer::rebound_optimize(double &final_cost) - { - iter_num_ = 0; - int start_id = order_; - // int end_id = this->cps_.size - order_; //Fixed end - int end_id = this->cps_.size; // Free end - variable_num_ = 3 * (end_id - start_id); - - ros::Time t0 = ros::Time::now(), t1, t2; - int restart_nums = 0, rebound_times = 0; - ; - bool flag_force_return, flag_occ, success; - new_lambda2_ = lambda2_; - constexpr int MAX_RESART_NUMS_SET = 3; - do - { - /* ---------- prepare ---------- */ - min_cost_ = std::numeric_limits::max(); - min_ellip_dist_ = INIT_min_ellip_dist_; - iter_num_ = 0; - flag_force_return = false; - flag_occ = false; - success = false; - - double q[variable_num_]; - memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0])); - - lbfgs::lbfgs_parameter_t lbfgs_params; - lbfgs::lbfgs_load_default_parameters(&lbfgs_params); - lbfgs_params.mem_size = 16; - lbfgs_params.max_iterations = 200; - lbfgs_params.g_epsilon = 0.01; - - /* ---------- optimize ---------- */ - t1 = ros::Time::now(); - int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRebound, NULL, BsplineOptimizer::earlyExit, this, &lbfgs_params); - t2 = ros::Time::now(); - double time_ms = (t2 - t1).toSec() * 1000; - double total_time_ms = (t2 - t0).toSec() * 1000; - - /* ---------- success temporary, check collision again ---------- */ - if (result == lbfgs::LBFGS_CONVERGENCE || - result == lbfgs::LBFGSERR_MAXIMUMITERATION || - result == lbfgs::LBFGS_ALREADY_MINIMIZED || - result == lbfgs::LBFGS_STOP) - { - //ROS_WARN("Solver error in planning!, return = %s", lbfgs::lbfgs_strerror(result)); - flag_force_return = false; - - /*** collision check, phase 1 ***/ - if ((min_ellip_dist_ != INIT_min_ellip_dist_) && (min_ellip_dist_ > swarm_clearance_)) - { - success = false; - restart_nums++; - initControlPoints(cps_.points, false); - new_lambda2_ *= 2; - - printf("\033[32miter(+1)=%d,time(ms)=%5.3f, swarm too close, keep optimizing\n\033[0m", iter_num_, time_ms); - - continue; - } - - /*** collision check, phase 2 ***/ - UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_); - double tm, tmp; - traj.getTimeSpan(tm, tmp); - double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution()); - for (double t = tm; t < tmp * 2 / 3; t += t_step) // Only check the closest 2/3 partition of the whole trajectory. - { - flag_occ = grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t)); - if (flag_occ) - { - //cout << "hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl; - - if (t <= bspline_interval_) // First 3 control points in obstacles! - { - // cout << cps_.points.col(1).transpose() << "\n" - // << cps_.points.col(2).transpose() << "\n" - // << cps_.points.col(3).transpose() << "\n" - // << cps_.points.col(4).transpose() << endl; - ROS_WARN("First 3 control points in obstacles! return false, t=%f", t); - return false; - } - - break; - } - } - - // cout << "XXXXXX" << ((cps_.points.col(cps_.points.cols()-1) + 4*cps_.points.col(cps_.points.cols()-2) + cps_.points.col(cps_.points.cols()-3))/6 - local_target_pt_).norm() << endl; - - /*** collision check, phase 3 ***/ -//#define USE_SECOND_CLEARENCE_CHECK -#ifdef USE_SECOND_CLEARENCE_CHECK - bool flag_cls_xyp, flag_cls_xyn, flag_cls_zp, flag_cls_zn; - Eigen::Vector3d start_end_vec = traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm); - Eigen::Vector3d offset_xy(-start_end_vec(0), start_end_vec(1), 0); - offset_xy.normalize(); - Eigen::Vector3d offset_z = start_end_vec.cross(offset_xy); - offset_z.normalize(); - offset_xy *= cps_.clearance / 2; - offset_z *= cps_.clearance / 2; - - Eigen::MatrixXd check_pts(cps_.points.rows(), cps_.points.cols()); - for (Eigen::Index i = 0; i < cps_.points.cols(); i++) - { - check_pts.col(i) = cps_.points.col(i); - check_pts(0, i) += offset_xy(0); - check_pts(1, i) += offset_xy(1); - check_pts(2, i) += offset_xy(2); - } - flag_cls_xyp = initControlPoints(check_pts, false).size() > 0; - for (Eigen::Index i = 0; i < cps_.points.cols(); i++) - { - check_pts(0, i) -= 2 * offset_xy(0); - check_pts(1, i) -= 2 * offset_xy(1); - check_pts(2, i) -= 2 * offset_xy(2); - } - flag_cls_xyn = initControlPoints(check_pts, false).size() > 0; - for (Eigen::Index i = 0; i < cps_.points.cols(); i++) - { - check_pts(0, i) += offset_xy(0) + offset_z(0); - check_pts(1, i) += offset_xy(1) + offset_z(1); - check_pts(2, i) += offset_xy(2) + offset_z(2); - } - flag_cls_zp = initControlPoints(check_pts, false).size() > 0; - for (Eigen::Index i = 0; i < cps_.points.cols(); i++) - { - check_pts(0, i) -= 2 * offset_z(0); - check_pts(1, i) -= 2 * offset_z(1); - check_pts(2, i) -= 2 * offset_z(2); - } - flag_cls_zn = initControlPoints(check_pts, false).size() > 0; - if ((flag_cls_xyp ^ flag_cls_xyn) || (flag_cls_zp ^ flag_cls_zn)) - flag_occ = true; -#endif - - if (!flag_occ) - { - // comment - // printf("\033[32miter(+1)=%d,time(ms)=%5.3f,total_t(ms)=%5.3f,cost=%5.3f\n\033[0m", iter_num_, time_ms, total_time_ms, final_cost); - success = true; - } - else // restart - { - restart_nums++; - initControlPoints(cps_.points, false); - new_lambda2_ *= 2; - // comment - // printf("\033[32miter(+1)=%d,time(ms)=%5.3f, collided, keep optimizing\n\033[0m", iter_num_, time_ms); - } - } - else if (result == lbfgs::LBFGSERR_CANCELED) - { - flag_force_return = true; - rebound_times++; - // comment - // cout << "iter=" << iter_num_ << ",time(ms)=" << time_ms << ",rebound." << endl; - } - else - { - ROS_WARN("Solver error. Return = %d, %s. Skip this planning.", result, lbfgs::lbfgs_strerror(result)); - // while (ros::ok()); - } - - } while ( - ((flag_occ || ((min_ellip_dist_ != INIT_min_ellip_dist_) && (min_ellip_dist_ > swarm_clearance_))) && restart_nums < MAX_RESART_NUMS_SET) || - (flag_force_return && force_stop_type_ == STOP_FOR_REBOUND && rebound_times <= 20)); - - return success; - } - - bool BsplineOptimizer::refine_optimize() - { - iter_num_ = 0; - int start_id = order_; - int end_id = this->cps_.points.cols() - order_; - variable_num_ = 3 * (end_id - start_id); - - double q[variable_num_]; - double final_cost; - - memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0])); - - double origin_lambda4 = lambda4_; - bool flag_safe = true; - int iter_count = 0; - do - { - lbfgs::lbfgs_parameter_t lbfgs_params; - lbfgs::lbfgs_load_default_parameters(&lbfgs_params); - lbfgs_params.mem_size = 16; - lbfgs_params.max_iterations = 200; - lbfgs_params.g_epsilon = 0.001; - - int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRefine, NULL, NULL, this, &lbfgs_params); - if (result == lbfgs::LBFGS_CONVERGENCE || - result == lbfgs::LBFGSERR_MAXIMUMITERATION || - result == lbfgs::LBFGS_ALREADY_MINIMIZED || - result == lbfgs::LBFGS_STOP) - { - //pass - } - else - { - ROS_ERROR("Solver error in refining!, return = %d, %s", result, lbfgs::lbfgs_strerror(result)); - } - - UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_); - double tm, tmp; - traj.getTimeSpan(tm, tmp); - double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution()); // Step size is defined as the maximum size that can passes throgth every gird. - for (double t = tm; t < tmp * 2 / 3; t += t_step) - { - if (grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t))) - { - // cout << "Refined traj hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl; - - Eigen::MatrixXd ref_pts(ref_pts_.size(), 3); - for (size_t i = 0; i < ref_pts_.size(); i++) - { - ref_pts.row(i) = ref_pts_[i].transpose(); - } - - flag_safe = false; - break; - } - } - - if (!flag_safe) - lambda4_ *= 2; - - iter_count++; - } while (!flag_safe && iter_count <= 0); - - lambda4_ = origin_lambda4; - - //cout << "iter_num_=" << iter_num_ << endl; - - return flag_safe; - } - - void BsplineOptimizer::combineCostRebound(const double *x, double *grad, double &f_combine, const int n) - { - // cout << "drone_id_=" << drone_id_ << endl; - // cout << "cps_.points.size()=" << cps_.points.size() << endl; - // cout << "n=" << n << endl; - // cout << "sizeof(x[0])=" << sizeof(x[0]) << endl; - - memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0])); - - /* ---------- evaluate cost and gradient ---------- */ - double f_smoothness, f_distance, f_feasibility /*, f_mov_objs*/, f_swarm, f_terminal; - - Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.size); - Eigen::MatrixXd g_distance = Eigen::MatrixXd::Zero(3, cps_.size); - Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.size); - // Eigen::MatrixXd g_mov_objs = Eigen::MatrixXd::Zero(3, cps_.size); - Eigen::MatrixXd g_swarm = Eigen::MatrixXd::Zero(3, cps_.size); - Eigen::MatrixXd g_terminal = Eigen::MatrixXd::Zero(3, cps_.size); - - calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness); - calcDistanceCostRebound(cps_.points, f_distance, g_distance, iter_num_, f_smoothness); - calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility); - // calcMovingObjCost(cps_.points, f_mov_objs, g_mov_objs); - calcSwarmCost(cps_.points, f_swarm, g_swarm); - calcTerminalCost(cps_.points, f_terminal, g_terminal); - - f_combine = lambda1_ * f_smoothness + new_lambda2_ * f_distance + lambda3_ * f_feasibility + new_lambda2_ * f_swarm + lambda2_ * f_terminal; - //f_combine = lambda1_ * f_smoothness + new_lambda2_ * f_distance + lambda3_ * f_feasibility + new_lambda2_ * f_mov_objs; - //printf("origin %f %f %f %f\n", f_smoothness, f_distance, f_feasibility, f_combine); - - Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + new_lambda2_ * g_distance + lambda3_ * g_feasibility + new_lambda2_ * g_swarm + lambda2_ * g_terminal; - //Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + new_lambda2_ * g_distance + lambda3_ * g_feasibility + new_lambda2_ * g_mov_objs; - memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0])); - } - - void BsplineOptimizer::combineCostRefine(const double *x, double *grad, double &f_combine, const int n) - { - - memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0])); - - /* ---------- evaluate cost and gradient ---------- */ - double f_smoothness, f_fitness, f_feasibility; - - Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.points.cols()); - Eigen::MatrixXd g_fitness = Eigen::MatrixXd::Zero(3, cps_.points.cols()); - Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.points.cols()); - - //time_satrt = ros::Time::now(); - - calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness); - calcFitnessCost(cps_.points, f_fitness, g_fitness); - calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility); - - /* ---------- convert to solver format...---------- */ - f_combine = lambda1_ * f_smoothness + lambda4_ * f_fitness + lambda3_ * f_feasibility; - // printf("origin %f %f %f %f\n", f_smoothness, f_fitness, f_feasibility, f_combine); - - Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + lambda4_ * g_fitness + lambda3_ * g_feasibility; - memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0])); - } - -} // namespace ego_planner \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/gradient_descent_optimizer.cpp b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/gradient_descent_optimizer.cpp deleted file mode 100644 index c3371d6c..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/gradient_descent_optimizer.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include - -#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; -} diff --git a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/uniform_bspline.cpp b/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/uniform_bspline.cpp deleted file mode 100644 index ff798f01..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/bspline_opt/src/uniform_bspline.cpp +++ /dev/null @@ -1,377 +0,0 @@ -#include "bspline_opt/uniform_bspline.h" -#include - -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 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 &point_set, - const vector &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 diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/CMakeLists.txt b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/CMakeLists.txt deleted file mode 100644 index 1eacae46..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/CMakeLists.txt +++ /dev/null @@ -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() diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/LICENSE b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/LICENSE deleted file mode 100644 index 9b02d46f..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/LICENSE +++ /dev/null @@ -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. diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/README.md b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/README.md deleted file mode 100644 index 2388e527..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/README.md +++ /dev/null @@ -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. - - - -![Example image](doc/demo.jpg) - - - -## 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 - - - - - - - - - - - - - - -``` - - - -## 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. \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/config/camera.yaml b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/config/camera.yaml deleted file mode 100644 index 556e5962..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/config/camera.yaml +++ /dev/null @@ -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 - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/config/default.yaml b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/config/default.yaml deleted file mode 100644 index 37a666d2..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/config/default.yaml +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/doc/demo.jpg b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/doc/demo.jpg deleted file mode 100644 index 690972dd..00000000 Binary files a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/doc/demo.jpg and /dev/null differ diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/doc/example.jpg b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/doc/example.jpg deleted file mode 100644 index d047c4fa..00000000 Binary files a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/doc/example.jpg and /dev/null differ diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/include/drone_detector/drone_detector.h b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/include/drone_detector/drone_detector.h deleted file mode 100644 index 271210ea..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/include/drone_detector/drone_detector.h +++ /dev/null @@ -1,156 +0,0 @@ -#pragma once -#include -#include -// ROS -#include -#include "std_msgs/String.h" -#include "std_msgs/Bool.h" -#include -#include -#include -// synchronize topic -#include -#include -#include -#include - -#include - -//include opencv and eigen -#include -#include -#include -#include -#include - -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 SyncPolicyDepthColorImagePose; - typedef std::shared_ptr> SynchronizerDepthColorImagePose; - typedef message_filters::sync_policies::ApproximateTime SyncPolicyDepthImagePose; - typedef std::shared_ptr> SynchronizerDepthImagePose; - - // std::shared_ptr> depth_img_sub_; - std::shared_ptr> colordepth_img_sub_; - std::shared_ptr> 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 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 */ diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/launch/drone_detect.launch b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/launch/drone_detect.launch deleted file mode 100644 index 6aa4a254..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/launch/drone_detect.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/launch/ros_package_template_overlying_params.launch b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/launch/ros_package_template_overlying_params.launch deleted file mode 100644 index 0ac3e223..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/launch/ros_package_template_overlying_params.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/package.xml b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/package.xml deleted file mode 100644 index bef32704..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - drone_detect - 0.1.0 - Detect other drones in depth image. - Jiangchao Zhu - BSD - Jiangchao Zhu - - - catkin - - boost - - eigen - roscpp - sensor_msgs - - roslint - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/src/drone_detect_node.cpp b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/src/drone_detect_node.cpp deleted file mode 100644 index 8e29362d..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/src/drone_detect_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#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; -} diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/src/drone_detector.cpp b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/src/drone_detector.cpp deleted file mode 100644 index d3bacd3d..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/src/drone_detector.cpp +++ /dev/null @@ -1,423 +0,0 @@ -#include "drone_detector/drone_detector.h" - -// STD -#include - -namespace detect { - -DroneDetector::DroneDetector(ros::NodeHandle& nodeHandle) - : nh_(nodeHandle) -{ - readParameters(); - - // depth_img_sub_.reset(new message_filters::Subscriber(nh_, "depth", 50, ros::TransportHints().tcpNoDelay())); - // colordepth_img_sub_.reset(new message_filters::Subscriber(nh_, "colordepth", 50)); - // camera_pos_sub_.reset(new message_filters::Subscriber(nh_, "camera_pose", 50)); - - my_odom_sub_ = nh_.subscribe("odometry", 100, &DroneDetector::rcvMyOdomCallback, this, ros::TransportHints().tcpNoDelay()); - depth_img_sub_ = nh_.subscribe("depth", 50, &DroneDetector::rcvDepthImgCallback, this, ros::TransportHints().tcpNoDelay()); - // sync_depth_color_img_pose_->registerCallback(boost::bind(&DroneDetector::rcvDepthColorCamPoseCallback, this, _1, _2, _3)); - - // drone0_odom_sub_ = nh_.subscribe("drone0", 50, &DroneDetector::rcvDrone0OdomCallback, this); - // drone1_odom_sub_ = nh_.subscribe("drone1", 50, &DroneDetector::rcvDrone1OdomCallback, this); - // drone2_odom_sub_ = nh_.subscribe("drone2", 50, &DroneDetector::rcvDrone2OdomCallback, this); - droneX_odom_sub_ = nh_.subscribe("/others_odom", 100, &DroneDetector::rcvDroneXOdomCallback, this, ros::TransportHints().tcpNoDelay()); - - new_depth_img_pub_ = nh_.advertise("new_depth_image", 50); - debug_depth_img_pub_ = nh_.advertise("debug_depth_image", 50); - - debug_info_pub_ = nh_.advertise("/debug_info", 50); - - cam2body_ << 0.0, 0.0, 1.0, 0.0, - -1.0, 0.0, 0.0, 0.0, - 0.0, -1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0; - - // init drone_pose_err_pub - for(int i = 0; i < max_drone_num_; i++) { - if(i != my_id_) - drone_pose_err_pub_[i] = nh_.advertise("drone"+std::to_string(i)+"to"+std::to_string(my_id_)+"_pose_err", 50); - } - - ROS_INFO("Successfully launched node."); -} - -DroneDetector::~DroneDetector() -{ -} - -void DroneDetector::readParameters() -{ - // camera params - nh_.getParam("cam_width", img_width_); - nh_.getParam("cam_height", img_height_); - nh_.getParam("cam_fx", fx_); - nh_.getParam("cam_fy", fy_); - nh_.getParam("cam_cx", cx_); - nh_.getParam("cam_cy", cy_); - - // - nh_.getParam("debug_flag", debug_flag_); - nh_.getParam("pixel_ratio", pixel_ratio_); - nh_.getParam("my_id", my_id_); - nh_.getParam("estimate/drone_width", drone_width_); - nh_.getParam("estimate/drone_height", drone_height_); - nh_.getParam("estimate/max_pose_error", max_pose_error_); - - max_pose_error2_ = max_pose_error_*max_pose_error_; -} - -// inline functions -inline double DroneDetector::getDist2(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2) -{ - double delta_x = p1(0)-p2(0); - double delta_y = p1(1)-p2(1); - double delta_z = p1(2)-p2(2); - return delta_x*delta_x+delta_y*delta_y+delta_z*delta_z; -} - -inline double DroneDetector::getDist2(const Eigen::Vector4d &p1, const Eigen::Vector4d &p2) -{ - double delta_x = p1(0)-p2(0); - double delta_y = p1(1)-p2(1); - double delta_z = p1(2)-p2(2); - return delta_x*delta_x+delta_y*delta_y+delta_z*delta_z; -} - -inline Eigen::Vector4d DroneDetector::depth2Pos(int u, int v, float depth) -{ - Eigen::Vector4d pose_in_camera; - pose_in_camera(0) = (u - cx_) * depth / fx_; - pose_in_camera(1) = (v - cy_) * depth / fy_; - pose_in_camera(2) = depth; - pose_in_camera(3) = 1.0; - return pose_in_camera; -} - -inline Eigen::Vector4d DroneDetector::depth2Pos(const Eigen::Vector2i &pixel, float depth) -{ - Eigen::Vector4d pose_in_camera; - pose_in_camera(0) = (pixel(0) - cx_) * depth / fx_; - pose_in_camera(1) = (pixel(1) - cy_) * depth / fy_; - pose_in_camera(2) = depth; - pose_in_camera(3) = 1.0; - return pose_in_camera; -} - -inline Eigen::Vector2i DroneDetector::pos2Depth(const Eigen::Vector4d &pose_in_camera) -{ - float depth = pose_in_camera(2); - Eigen::Vector2i pixel; - pixel(0) = pose_in_camera(0) * fx_ / depth + cx_ + 0.5; - pixel(1) = pose_in_camera(1) * fy_ / depth + cy_ + 0.5; - return pixel; -} - -inline bool DroneDetector::isInSensorRange(const Eigen::Vector2i &pixel) -{ - if (pixel(0)>=0 && pixel(1) >= 0 && pixel(0) <= img_width_ && pixel(1) <= img_height_) return true; - else - return false; -} - -void DroneDetector::rcvMyOdomCallback(const nav_msgs::Odometry& odom) -{ - my_odom_ = odom; - Eigen::Matrix4d body2world = Eigen::Matrix4d::Identity(); - - my_pose_world_(0) = odom.pose.pose.position.x; - my_pose_world_(1) = odom.pose.pose.position.y; - my_pose_world_(2) = odom.pose.pose.position.z; - my_pose_world_(3) = 1.0; - my_attitude_world_.x() = odom.pose.pose.orientation.x; - my_attitude_world_.y() = odom.pose.pose.orientation.y; - my_attitude_world_.z() = odom.pose.pose.orientation.z; - my_attitude_world_.w() = odom.pose.pose.orientation.w; - body2world.block<3,3>(0,0) = my_attitude_world_.toRotationMatrix(); - body2world(0,3) = my_pose_world_(0); - body2world(1,3) = my_pose_world_(1); - body2world(2,3) = my_pose_world_(2); - - //convert to cam pose - cam2world_ = body2world * cam2body_; - cam2world_quat_ = cam2world_.block<3,3>(0,0); - - // my_last_odom_stamp_ = odom.header.stamp; - - // my_last_pose_world_(0) = odom.pose.pose.position.x; - // my_last_pose_world_(1) = odom.pose.pose.position.y; - // my_last_pose_world_(2) = odom.pose.pose.position.z; - // my_last_pose_world_(3) = 1.0; - - //publish tf - // static tf::TransformBroadcaster br; - // tf::Transform transform; - // transform.setOrigin( tf::Vector3(cam2world(0,3), cam2world(1,3), cam2world(2,3) )); - // transform.setRotation(tf::Quaternion(cam2world_quat.x(), cam2world_quat.y(), cam2world_quat.z(), cam2world_quat.w())); - // br.sendTransform(tf::StampedTransform(transform, my_last_odom_stamp, "world", "camera")); - //publish transform from world frame to quadrotor frame. -} -void DroneDetector::rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img) -{ - /* get depth image */ - cv_bridge::CvImagePtr cv_ptr; - cv_ptr = cv_bridge::toCvCopy(depth_img, depth_img->encoding); - cv_ptr->image.copyTo(depth_img_); - - debug_start_time_ = ros::Time::now(); - - Eigen::Vector2i true_pixel[max_drone_num_]; - for (int i = 0; i < max_drone_num_; i++) { - if (in_depth_[i]) { - detect(i, true_pixel[i]); - } - } - - cv_bridge::CvImage out_msg; - for (int i = 0; i < max_drone_num_; i++) { - if (in_depth_[i]) { - // erase hit pixels in depth - for(int k = 0; k < int(hit_pixels_[i].size()); k++) { - // depth_img_.at(hit_pixels_[i][k](1), hit_pixels_[i][k](0)) = 0; - uint16_t *row_ptr; - row_ptr = depth_img_.ptr(hit_pixels_[i][k](1)); - (*(row_ptr+hit_pixels_[i][k](0))) = 0.0; - } - } - } - debug_end_time_ = ros::Time::now(); - // ROS_WARN("cost_total_time = %lf", (debug_end_time_ - debug_start_time_).toSec()*1000.0); - out_msg.header = depth_img->header; - out_msg.encoding = depth_img->encoding; - out_msg.image = depth_img_.clone(); - new_depth_img_pub_.publish(out_msg.toImageMsg()); - - std_msgs::String msg; - std::stringstream ss; - if(debug_flag_) { - for (int i = 0; i < max_drone_num_; i++) { - if (in_depth_[i]) { - // add bound box in colormap - // cv::Rect rect(_bbox_lu.x, _bbox_lu.y, _bbox_rd.x, _bbox_rd.y);//左上坐标（x,y）和矩形的长(x)宽(y) - cv::rectangle(depth_img_, cv::Rect(searchbox_lu_[i], searchbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0); - cv::rectangle(depth_img_, cv::Rect(boundingbox_lu_[i], boundingbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0); - if (debug_detect_result_[i] == 1) { - ss << "no enough " << hit_pixels_[i].size(); - } else if(debug_detect_result_[i] == 2) { - ss << "success"; - } - } else { - ss << "no detect"; - } - } - out_msg.header = depth_img->header; - out_msg.encoding = depth_img->encoding; - out_msg.image = depth_img_.clone(); - debug_depth_img_pub_.publish(out_msg.toImageMsg()); - msg.data = ss.str(); - debug_info_pub_.publish(msg); - } -} - -void DroneDetector::rcvDroneOdomCallbackBase(const nav_msgs::Odometry& odom, int drone_id) -{ - if (drone_id == my_id_) { - return; - } - Eigen::Matrix4d drone2world = Eigen::Matrix4d::Identity(); - drone_pose_world_[drone_id](0) = odom.pose.pose.position.x; - drone_pose_world_[drone_id](1) = odom.pose.pose.position.y; - drone_pose_world_[drone_id](2) = odom.pose.pose.position.z; - drone_pose_world_[drone_id](3) = 1.0; - - drone_attitude_world_[drone_id].x() = odom.pose.pose.orientation.x; - drone_attitude_world_[drone_id].y() = odom.pose.pose.orientation.y; - drone_attitude_world_[drone_id].z() = odom.pose.pose.orientation.z; - drone_attitude_world_[drone_id].w() = odom.pose.pose.orientation.w; - drone2world.block<3,3>(0,0) = drone_attitude_world_[drone_id].toRotationMatrix(); - - drone2world(0,3) = drone_pose_world_[drone_id](0); - drone2world(1,3) = drone_pose_world_[drone_id](1); - drone2world(2,3) = drone_pose_world_[drone_id](2); - - drone_pose_cam_[drone_id] = cam2world_.inverse() * drone_pose_world_[drone_id]; - // if the drone is in sensor range - drone_ref_pixel_[drone_id] = pos2Depth(drone_pose_cam_[drone_id]); - if (drone_pose_cam_[drone_id](2) > 0 && isInSensorRange(drone_ref_pixel_[drone_id])) { - in_depth_[drone_id] = true; - } else { - in_depth_[drone_id] = false; - debug_detect_result_[drone_id] = 0; - } -} - -void DroneDetector::rcvDrone0OdomCallback(const nav_msgs::Odometry& odom) -{ - rcvDroneOdomCallbackBase(odom, 0); -} - -void DroneDetector::rcvDrone1OdomCallback(const nav_msgs::Odometry& odom) -{ - rcvDroneOdomCallbackBase(odom, 1); -} - -void DroneDetector::rcvDrone2OdomCallback(const nav_msgs::Odometry& odom) -{ - rcvDroneOdomCallbackBase(odom, 2); -} - -void DroneDetector::rcvDroneXOdomCallback(const nav_msgs::Odometry& odom) -{ - std::string numstr = odom.child_frame_id.substr(6); - try - { - int drone_id = std::stoi(numstr); - rcvDroneOdomCallbackBase(odom, drone_id); - } - catch(const std::exception& e) - { - std::cout << e.what() << '\n'; - } -} - -bool DroneDetector::countPixel(int drone_id, Eigen::Vector2i &true_pixel, Eigen::Vector4d &true_pose_cam) -{ - boundingbox_lu_[drone_id].x = img_width_; - boundingbox_rd_[drone_id].x = 0; - boundingbox_lu_[drone_id].y = img_height_; - boundingbox_rd_[drone_id].y = 0; - - valid_pixel_cnt_[drone_id] = 0; - hit_pixels_[drone_id].clear(); - - Eigen::Vector2i tmp_pixel; - Eigen::Vector4d tmp_pose_cam; - int search_radius = 2*max_pose_error_*fx_/drone_pose_cam_[drone_id](2); - float depth; - searchbox_lu_[drone_id].x = drone_ref_pixel_[drone_id](0) - search_radius; - searchbox_lu_[drone_id].y = drone_ref_pixel_[drone_id](1) - search_radius; - searchbox_rd_[drone_id].x = drone_ref_pixel_[drone_id](0) + search_radius; - searchbox_rd_[drone_id].y = drone_ref_pixel_[drone_id](1) + search_radius; - // check the tmp_p around ref_pixel - for(int i = -search_radius; i <= search_radius; i++) - for(int j = -search_radius; j <= search_radius; j++) - { - tmp_pixel(0) = drone_ref_pixel_[drone_id](0) + j; - tmp_pixel(1) = drone_ref_pixel_[drone_id](1) + i; - if(tmp_pixel(0) < 0 || tmp_pixel(0) >= img_width_ || tmp_pixel(1) < 0 || tmp_pixel(1) >= img_height_) - continue; - // depth = depth_img_.at(tmp_pixel(1), tmp_pixel(0)); - uint16_t *row_ptr; - row_ptr = depth_img_.ptr(tmp_pixel(1)); - depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; - // ROS_WARN("depth = %lf", depth); - // get tmp_pose in cam frame - tmp_pose_cam = depth2Pos(tmp_pixel(0), tmp_pixel(1), depth); - double dist2 = getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]); - // ROS_WARN("dist2 = %lf", dist2); - if (dist2 < max_pose_error2_) { - valid_pixel_cnt_[drone_id]++; - hit_pixels_[drone_id].push_back(tmp_pixel); - boundingbox_lu_[drone_id].x = tmp_pixel(0) < boundingbox_lu_[drone_id].x ? tmp_pixel(0) : boundingbox_lu_[drone_id].x; - boundingbox_lu_[drone_id].y = tmp_pixel(1) < boundingbox_lu_[drone_id].y ? tmp_pixel(1) : boundingbox_lu_[drone_id].y; - boundingbox_rd_[drone_id].x = tmp_pixel(0) > boundingbox_rd_[drone_id].x ? tmp_pixel(0) : boundingbox_rd_[drone_id].x; - boundingbox_rd_[drone_id].y = tmp_pixel(1) > boundingbox_rd_[drone_id].y ? tmp_pixel(1) : boundingbox_rd_[drone_id].y; - } - } - pixel_threshold_ = (drone_width_*fx_/drone_pose_cam_[drone_id](2)) * (drone_height_*fy_/drone_pose_cam_[drone_id](2))*pixel_ratio_; - if (valid_pixel_cnt_[drone_id] > pixel_threshold_) { - int step = 1, size = (boundingbox_rd_[drone_id].y-boundingbox_lu_[drone_id].y) < (boundingbox_rd_[drone_id].x-boundingbox_lu_[drone_id].x) ? (boundingbox_rd_[drone_id].y-boundingbox_lu_[drone_id].y) : (boundingbox_rd_[drone_id].x-boundingbox_lu_[drone_id].x); - int init_x = (boundingbox_lu_[drone_id].x+boundingbox_rd_[drone_id].x)/2, init_y = (boundingbox_lu_[drone_id].y+boundingbox_rd_[drone_id].y)/2; - int x_flag = 1, y_flag = 1; - int x_idx = 0, y_idx = 0; - uint16_t *row_ptr; - row_ptr = depth_img_.ptr(tmp_pixel(1)); - depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; - tmp_pose_cam = depth2Pos(init_x, init_y, depth); - if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){ - true_pixel(0) = init_x; - true_pixel(1) = init_y; - true_pose_cam = tmp_pose_cam; - return true; - } - while(step(tmp_pixel(1)); - depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; - tmp_pose_cam = depth2Pos(init_x, init_y, depth); - if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_) { - true_pixel(0) = init_x; - true_pixel(1) = init_y; - true_pose_cam = tmp_pose_cam; - return true; - } - x_idx++; - } - x_idx = 0; - x_flag = -x_flag; - while(y_idx(tmp_pixel(1)); - depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; - tmp_pose_cam = depth2Pos(init_x, init_y, depth); - if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){ - true_pixel(0) = init_x; - true_pixel(1) = init_y; - true_pose_cam = tmp_pose_cam; - return true; - } - y_idx++; - } - y_idx = 0; - y_flag = -y_flag; - step++; - } - while(x_idx(tmp_pixel(1)); - depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; - tmp_pose_cam = depth2Pos(init_x, init_y, depth); - if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){ - true_pixel(0) = init_x; - true_pixel(1) = init_y; - true_pose_cam = tmp_pose_cam; - return true; - } - x_idx++; - } - } - return false; -} - -void DroneDetector::detect(int drone_id, Eigen::Vector2i &true_pixel) -{ - Eigen::Vector4d true_pose_cam, pose_error; - bool found = countPixel(drone_id, true_pixel, true_pose_cam); - if (found) { - // ROS_WARN("FOUND"); - pose_error = cam2world_*true_pose_cam - drone_pose_world_[drone_id]; - debug_detect_result_[drone_id] = 2; - - geometry_msgs::PoseStamped out_msg; - out_msg.header.stamp = my_last_camera_stamp_; - out_msg.header.frame_id = "/drone_detect"; - out_msg.pose.position.x = pose_error(0); - out_msg.pose.position.y = pose_error(1); - out_msg.pose.position.z = pose_error(2); - drone_pose_err_pub_[drone_id].publish(out_msg); - - } else { - // ROS_WARN("NOT FOUND"); - debug_detect_result_[drone_id] = 1; - } -} - -void DroneDetector::test() { - ROS_WARN("my_id = %d", my_id_); -} - -} /* namespace */ diff --git a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/test/test_drone_detector.cpp b/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/test/test_drone_detector.cpp deleted file mode 100644 index 2fc9dd58..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/drone_detect/test/test_drone_detector.cpp +++ /dev/null @@ -1,10 +0,0 @@ -// gtest -#include - -// 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(); -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/ego_planner.md b/src/无人机端代码/Modules/ego_planner_swarm/ego_planner.md deleted file mode 100644 index 747e4000..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/ego_planner.md +++ /dev/null @@ -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 - - -## 运行 - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/path_searching/CMakeLists.txt b/src/无人机端代码/Modules/ego_planner_swarm/path_searching/CMakeLists.txt deleted file mode 100644 index 13f1f661..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/path_searching/CMakeLists.txt +++ /dev/null @@ -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} - ) diff --git a/src/无人机端代码/Modules/ego_planner_swarm/path_searching/include/path_searching/dyn_a_star.h b/src/无人机端代码/Modules/ego_planner_swarm/path_searching/include/path_searching/dyn_a_star.h deleted file mode 100644 index 388668b2..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/path_searching/include/path_searching/dyn_a_star.h +++ /dev/null @@ -1,115 +0,0 @@ -#ifndef _DYN_A_STAR_H_ -#define _DYN_A_STAR_H_ - -#include -#include -#include -#include -#include -#include - -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 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 gridPath_; - - GridNodePtr ***GridNodeMap_; - std::priority_queue, NodeComparator> openSet_; - - int rounds_{0}; - -public: - typedef std::shared_ptr 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 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() * 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() + 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 diff --git a/src/无人机端代码/Modules/ego_planner_swarm/path_searching/package.xml b/src/无人机端代码/Modules/ego_planner_swarm/path_searching/package.xml deleted file mode 100644 index 0394974a..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/path_searching/package.xml +++ /dev/null @@ -1,71 +0,0 @@ - - - path_searching - 0.0.0 - The path_searching package - - - - - iszhouxin - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - plan_env - roscpp - rospy - std_msgs - plan_env - roscpp - rospy - std_msgs - plan_env - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/path_searching/src/dyn_a_star.cpp b/src/无人机端代码/Modules/ego_planner_swarm/path_searching/src/dyn_a_star.cpp deleted file mode 100644 index 744daa94..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/path_searching/src/dyn_a_star.cpp +++ /dev/null @@ -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 AStar::retrievePath(GridNodePtr current) -{ - vector 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, 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 AStar::getPath() -{ - vector path; - - for (auto ptr : gridPath_) - path.push_back(Index2Coord(ptr->index)); - - reverse(path.begin(), path.end()); - return path; -} diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/CMakeLists.txt b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/CMakeLists.txt deleted file mode 100644 index 701e9af2..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(plan_env) - -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(OpenCV REQUIRED) - - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - visualization_msgs - cv_bridge - message_filters -) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES plan_env - CATKIN_DEPENDS roscpp std_msgs - DEPENDS OpenCV -# DEPENDS system_lib -) - -include_directories( - SYSTEM - include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} -) - -link_directories(${PCL_LIBRARY_DIRS}) - -add_library( plan_env - src/grid_map.cpp - src/raycast.cpp - src/obj_predictor.cpp - ) -target_link_libraries( plan_env - ${catkin_LIBRARIES} - ${PCL_LIBRARIES} - ${OpenCV_LIBS} - ) - -add_executable(obj_generator - src/obj_generator.cpp -) -target_link_libraries(obj_generator - ${catkin_LIBRARIES} - ) diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/grid_map.h b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/grid_map.h deleted file mode 100644 index bba2603d..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/grid_map.h +++ /dev/null @@ -1,804 +0,0 @@ -#ifndef _GRID_MAP_H -#define _GRID_MAP_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include - -#define logit(x) (log((x) / (1 - (x)))) - -using namespace std; - -// voxel hashing -template -struct matrix_hash : std::unary_function { - std::size_t operator()(T const& matrix) const { - size_t seed = 0; - for (size_t i = 0; i < matrix.size(); ++i) { - auto elem = *(matrix.data() + i); - seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - } - return seed; - } -}; - -// constant parameters - -struct MappingParameters { - - /* map properties */ - Eigen::Vector3d map_origin_, map_size_; - Eigen::Vector3d map_min_boundary_, map_max_boundary_; // map range in pos - Eigen::Vector3i map_voxel_num_; // map range in index - Eigen::Vector3d local_update_range_; - double resolution_, resolution_inv_; - double obstacles_inflation_; - string frame_id_; - int pose_type_; - - /* camera parameters */ - double cx_, cy_, fx_, fy_; - - /* time out */ - double odom_depth_timeout_; - - /* depth image projection filtering */ - double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_; - int depth_filter_margin_; - bool use_depth_filter_; - double k_depth_scaling_factor_; - int skip_pixel_; - - /* raycasting */ - double p_hit_, p_miss_, p_min_, p_max_, p_occ_; // occupancy probability - double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_, - min_occupancy_log_; // logit of occupancy probability - double min_ray_length_, max_ray_length_; // range of doing raycasting - - /* local map update and clear */ - int local_map_margin_; - - /* visualization and computation time display */ - double visualization_truncate_height_, virtual_ceil_height_, ground_height_, virtual_ceil_yp_, virtual_ceil_yn_; - bool show_occ_time_; - - /* active mapping */ - double unknown_flag_; -}; - -// intermediate mapping data for fusion - -struct MappingData { - // main map data, occupancy of each voxel and Euclidean distance - - std::vector occupancy_buffer_; - std::vector occupancy_buffer_inflate_; - - // camera position and pose data - - Eigen::Vector3d camera_pos_, last_camera_pos_; - Eigen::Matrix3d camera_r_m_, last_camera_r_m_; - Eigen::Matrix4d cam2body_; - - // depth image data - - cv::Mat depth_image_, last_depth_image_; - int image_cnt_; - - // flags of map state - - bool occ_need_update_, local_updated_; - bool has_first_depth_; - bool has_odom_, has_cloud_; - - // odom_depth_timeout_ - ros::Time last_occ_update_time_; - bool flag_depth_odom_timeout_; - bool flag_use_depth_fusion; - - // depth image projected point cloud - - vector proj_points_; - int proj_points_cnt; - - // flag buffers for speeding up raycasting - - vector count_hit_, count_hit_and_miss_; - vector flag_traverse_, flag_rayend_; - char raycast_num_; - queue cache_voxel_; - - // range of updating grid - - Eigen::Vector3i local_bound_min_, local_bound_max_; - - // computation time - - double fuse_time_, max_fuse_time_; - int update_num_; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -class GridMap { -public: - GridMap() {} - ~GridMap() {} - - enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 }; - - // occupancy map management - void resetBuffer(); - void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max); - - inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id); - inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos); - inline int toAddress(const Eigen::Vector3i& id); - inline int toAddress(int& x, int& y, int& z); - inline bool isInMap(const Eigen::Vector3d& pos); - inline bool isInMap(const Eigen::Vector3i& idx); - - inline void setOccupancy(Eigen::Vector3d pos, double occ = 1); - inline void setOccupied(Eigen::Vector3d pos); - inline int getOccupancy(Eigen::Vector3d pos); - inline int getOccupancy(Eigen::Vector3i id); - inline int getInflateOccupancy(Eigen::Vector3d pos); - - inline void boundIndex(Eigen::Vector3i& id); - inline bool isUnknown(const Eigen::Vector3i& id); - inline bool isUnknown(const Eigen::Vector3d& pos); - inline bool isKnownFree(const Eigen::Vector3i& id); - inline bool isKnownOccupied(const Eigen::Vector3i& id); - - void initMap(ros::NodeHandle& nh); - - void publishMap(); - void publishMapInflate(bool all_info = false); - - void publishDepth(); - - bool hasDepthObservation(); - bool odomValid(); - void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size); - inline double getResolution(); - Eigen::Vector3d getOrigin(); - int getVoxelNum(); - bool getOdomDepthTimeout() { return md_.flag_depth_odom_timeout_; } - - typedef std::shared_ptr Ptr; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -private: - MappingParameters mp_; - MappingData md_; - - // get depth image and camera pose - void depthPoseCallback(const sensor_msgs::ImageConstPtr& img, - const geometry_msgs::PoseStampedConstPtr& pose); - void extrinsicCallback(const nav_msgs::OdometryConstPtr& odom); - void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom); - void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img); - void odomCallback(const nav_msgs::OdometryConstPtr& odom); - - // update occupancy by raycasting - void updateOccupancyCallback(const ros::TimerEvent& /*event*/); - void visCallback(const ros::TimerEvent& /*event*/); - - // main update process - void projectDepthImage(); - void raycastProcess(); - void clearAndInflateLocalMap(); - - inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector& pts); - int setCacheOccupancy(Eigen::Vector3d pos, int occ); - Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt); - - // typedef message_filters::sync_policies::ExactTime SyncPolicyImageOdom; typedef - // message_filters::sync_policies::ExactTime SyncPolicyImagePose; - typedef message_filters::sync_policies::ApproximateTime - SyncPolicyImageOdom; - typedef message_filters::sync_policies::ApproximateTime - SyncPolicyImagePose; - typedef shared_ptr> SynchronizerImagePose; - typedef shared_ptr> SynchronizerImageOdom; - - ros::NodeHandle node_; - shared_ptr> depth_sub_; - shared_ptr> pose_sub_; - shared_ptr> odom_sub_; - SynchronizerImagePose sync_image_pose_; - SynchronizerImageOdom sync_image_odom_; - - ros::Subscriber indep_cloud_sub_, indep_odom_sub_, extrinsic_sub_; - ros::Publisher map_pub_, map_inf_pub_; - ros::Timer occ_timer_, vis_timer_; - - // - uniform_real_distribution rand_noise_; - normal_distribution rand_noise2_; - default_random_engine eng_; -}; - -/* ============================== definition of inline function - * ============================== */ - -inline int GridMap::toAddress(const Eigen::Vector3i& id) { - return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2); -} - -inline int GridMap::toAddress(int& x, int& y, int& z) { - return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z; -} - -inline void GridMap::boundIndex(Eigen::Vector3i& id) { - Eigen::Vector3i id1; - id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0); - id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0); - id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0); - id = id1; -} - -inline bool GridMap::isUnknown(const Eigen::Vector3i& id) { - Eigen::Vector3i id1 = id; - boundIndex(id1); - return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3; -} - -inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) { - Eigen::Vector3i idc; - posToIndex(pos, idc); - return isUnknown(idc); -} - -inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) { - Eigen::Vector3i id1 = id; - boundIndex(id1); - int adr = toAddress(id1); - - // return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && - // md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_; - return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0; -} - -inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) { - Eigen::Vector3i id1 = id; - boundIndex(id1); - int adr = toAddress(id1); - - return md_.occupancy_buffer_inflate_[adr] == 1; -} - -inline void GridMap::setOccupied(Eigen::Vector3d pos) { - if (!isInMap(pos)) return; - - Eigen::Vector3i id; - posToIndex(pos, id); - - md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + - id(1) * mp_.map_voxel_num_(2) + id(2)] = 1; -} - -inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) { - if (occ != 1 && occ != 0) { - cout << "occ value error!" << endl; - return; - } - - if (!isInMap(pos)) return; - - Eigen::Vector3i id; - posToIndex(pos, id); - - md_.occupancy_buffer_[toAddress(id)] = occ; -} - -inline int GridMap::getOccupancy(Eigen::Vector3d pos) { - if (!isInMap(pos)) return -1; - - Eigen::Vector3i id; - posToIndex(pos, id); - - return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0; -} - -inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) { - if (!isInMap(pos)) return -1; - - Eigen::Vector3i id; - posToIndex(pos, id); - - return int(md_.occupancy_buffer_inflate_[toAddress(id)]); -} - -inline int GridMap::getOccupancy(Eigen::Vector3i id) { - if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) || - id(2) < 0 || id(2) >= mp_.map_voxel_num_(2)) - return -1; - - return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0; -} - -inline bool GridMap::isInMap(const Eigen::Vector3d& pos) { - if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 || - pos(2) < mp_.map_min_boundary_(2) + 1e-4) { - // cout << "less than min range!" << endl; - return false; - } - if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 || - pos(2) > mp_.map_max_boundary_(2) - 1e-4) { - return false; - } - return true; -} - -inline bool GridMap::isInMap(const Eigen::Vector3i& idx) { - if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) { - return false; - } - if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 || - idx(2) > mp_.map_voxel_num_(2) - 1) { - return false; - } - return true; -} - -inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) { - for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_); -} - -inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) { - for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i); -} - -inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector& pts) { - int num = 0; - - /* ---------- + shape inflate ---------- */ - // for (int x = -step; x <= step; ++x) - // { - // if (x == 0) - // continue; - // pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2)); - // } - // for (int y = -step; y <= step; ++y) - // { - // if (y == 0) - // continue; - // pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2)); - // } - // for (int z = -1; z <= 1; ++z) - // { - // pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z); - // } - - /* ---------- all inflate ---------- */ - for (int x = -step; x <= step; ++x) - for (int y = -step; y <= step; ++y) - for (int z = -step; z <= step; ++z) { - pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z); - } -} - -inline double GridMap::getResolution() { return mp_.resolution_; } - -#endif - -// #ifndef _GRID_MAP_H -// #define _GRID_MAP_H - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include -// #include -// #include - -// #include -// #include -// #include -// #include - -// #include - -// #define logit(x) (log((x) / (1 - (x)))) - -// using namespace std; - -// // voxel hashing -// template -// struct matrix_hash : std::unary_function { -// std::size_t operator()(T const& matrix) const { -// size_t seed = 0; -// for (size_t i = 0; i < matrix.size(); ++i) { -// auto elem = *(matrix.data() + i); -// seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); -// } -// return seed; -// } -// }; - -// // constant parameters - -// struct MappingParameters { - -// /* map properties */ -// Eigen::Vector3d map_origin_, map_size_; -// Eigen::Vector3d map_min_boundary_, map_max_boundary_; // map range in pos -// Eigen::Vector3i map_voxel_num_; // map range in index -// Eigen::Vector3d local_update_range_; -// double resolution_, resolution_inv_; -// double obstacles_inflation_; -// string frame_id_; -// int pose_type_; - -// /* camera parameters */ -// double cx_, cy_, fx_, fy_; - -// /* depth image projection filtering */ -// double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_; -// int depth_filter_margin_; -// bool use_depth_filter_; -// double k_depth_scaling_factor_; -// int skip_pixel_; - -// /* raycasting */ -// double p_hit_, p_miss_, p_min_, p_max_, p_occ_; // occupancy probability -// double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_, -// min_occupancy_log_; // logit of occupancy probability -// double min_ray_length_, max_ray_length_; // range of doing raycasting - -// /* local map update and clear */ -// int local_map_margin_; - -// /* visualization and computation time display */ -// double visualization_truncate_height_, virtual_ceil_height_, ground_height_; -// bool show_occ_time_; - -// /* active mapping */ -// double unknown_flag_; -// }; - -// // intermediate mapping data for fusion - -// struct MappingData { -// // main map data, occupancy of each voxel and Euclidean distance - -// std::vector occupancy_buffer_; -// std::vector occupancy_buffer_inflate_; - -// // camera position and pose data - -// Eigen::Vector3d camera_pos_, last_camera_pos_; -// Eigen::Quaterniond camera_q_, last_camera_q_; - -// // depth image data - -// cv::Mat depth_image_, last_depth_image_; -// int image_cnt_; - -// // flags of map state - -// bool occ_need_update_, local_updated_; -// bool has_first_depth_; -// bool has_odom_, has_cloud_; - -// // depth image projected point cloud - -// vector proj_points_; -// int proj_points_cnt; - -// // flag buffers for speeding up raycasting - -// vector count_hit_, count_hit_and_miss_; -// vector flag_traverse_, flag_rayend_; -// char raycast_num_; -// queue cache_voxel_; - -// // range of updating grid - -// Eigen::Vector3i local_bound_min_, local_bound_max_; - -// // computation time - -// double fuse_time_, max_fuse_time_; -// int update_num_; - -// EIGEN_MAKE_ALIGNED_OPERATOR_NEW -// }; - -// class GridMap { -// public: -// GridMap() {} -// ~GridMap() {} - -// enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 }; - -// // occupancy map management -// void resetBuffer(); -// void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max); - -// inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id); -// inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos); -// inline int toAddress(const Eigen::Vector3i& id); -// inline int toAddress(int& x, int& y, int& z); -// inline bool isInMap(const Eigen::Vector3d& pos); -// inline bool isInMap(const Eigen::Vector3i& idx); - -// inline void setOccupancy(Eigen::Vector3d pos, double occ = 1); -// inline void setOccupied(Eigen::Vector3d pos); -// inline int getOccupancy(Eigen::Vector3d pos); -// inline int getOccupancy(Eigen::Vector3i id); -// inline int getInflateOccupancy(Eigen::Vector3d pos); - -// inline void boundIndex(Eigen::Vector3i& id); -// inline bool isUnknown(const Eigen::Vector3i& id); -// inline bool isUnknown(const Eigen::Vector3d& pos); -// inline bool isKnownFree(const Eigen::Vector3i& id); -// inline bool isKnownOccupied(const Eigen::Vector3i& id); - -// void initMap(ros::NodeHandle& nh); - -// void publishMap(); -// void publishMapInflate(bool all_info = false); - -// void publishUnknown(); -// void publishDepth(); - -// bool hasDepthObservation(); -// bool odomValid(); -// void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size); -// inline double getResolution(); -// Eigen::Vector3d getOrigin(); -// int getVoxelNum(); - -// typedef std::shared_ptr Ptr; - -// EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -// private: -// MappingParameters mp_; -// MappingData md_; - -// // get depth image and camera pose -// void depthPoseCallback(const sensor_msgs::ImageConstPtr& img, -// const geometry_msgs::PoseStampedConstPtr& pose); -// void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom); -// void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img); -// void odomCallback(const nav_msgs::OdometryConstPtr& odom); - -// // update occupancy by raycasting -// void updateOccupancyCallback(const ros::TimerEvent& /*event*/); -// void visCallback(const ros::TimerEvent& /*event*/); - -// // main update process -// void projectDepthImage(); -// void raycastProcess(); -// void clearAndInflateLocalMap(); - -// inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector& pts); -// int setCacheOccupancy(Eigen::Vector3d pos, int occ); -// Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt); - -// // typedef message_filters::sync_policies::ExactTime SyncPolicyImageOdom; typedef -// // message_filters::sync_policies::ExactTime SyncPolicyImagePose; -// typedef message_filters::sync_policies::ApproximateTime -// SyncPolicyImageOdom; -// typedef message_filters::sync_policies::ApproximateTime -// SyncPolicyImagePose; -// typedef shared_ptr> SynchronizerImagePose; -// typedef shared_ptr> SynchronizerImageOdom; - -// ros::NodeHandle node_; -// shared_ptr> depth_sub_; -// shared_ptr> pose_sub_; -// shared_ptr> odom_sub_; -// SynchronizerImagePose sync_image_pose_; -// SynchronizerImageOdom sync_image_odom_; - -// ros::Subscriber indep_cloud_sub_, indep_odom_sub_; -// ros::Publisher map_pub_, map_inf_pub_; -// ros::Publisher unknown_pub_; -// ros::Timer occ_timer_, vis_timer_; - -// // -// uniform_real_distribution rand_noise_; -// normal_distribution rand_noise2_; -// default_random_engine eng_; -// }; - -// /* ============================== definition of inline function -// * ============================== */ - -// inline int GridMap::toAddress(const Eigen::Vector3i& id) { -// return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2); -// } - -// inline int GridMap::toAddress(int& x, int& y, int& z) { -// return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z; -// } - -// inline void GridMap::boundIndex(Eigen::Vector3i& id) { -// Eigen::Vector3i id1; -// id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0); -// id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0); -// id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0); -// id = id1; -// } - -// inline bool GridMap::isUnknown(const Eigen::Vector3i& id) { -// Eigen::Vector3i id1 = id; -// boundIndex(id1); -// return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3; -// } - -// inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) { -// Eigen::Vector3i idc; -// posToIndex(pos, idc); -// return isUnknown(idc); -// } - -// inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) { -// Eigen::Vector3i id1 = id; -// boundIndex(id1); -// int adr = toAddress(id1); - -// // return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && -// // md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_; -// return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0; -// } - -// inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) { -// Eigen::Vector3i id1 = id; -// boundIndex(id1); -// int adr = toAddress(id1); - -// return md_.occupancy_buffer_inflate_[adr] == 1; -// } - -// inline void GridMap::setOccupied(Eigen::Vector3d pos) { -// if (!isInMap(pos)) return; - -// Eigen::Vector3i id; -// posToIndex(pos, id); - -// md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + -// id(1) * mp_.map_voxel_num_(2) + id(2)] = 1; -// } - -// inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) { -// if (occ != 1 && occ != 0) { -// cout << "occ value error!" << endl; -// return; -// } - -// if (!isInMap(pos)) return; - -// Eigen::Vector3i id; -// posToIndex(pos, id); - -// md_.occupancy_buffer_[toAddress(id)] = occ; -// } - -// inline int GridMap::getOccupancy(Eigen::Vector3d pos) { -// if (!isInMap(pos)) return -1; - -// Eigen::Vector3i id; -// posToIndex(pos, id); - -// return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0; -// } - -// inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) { -// if (!isInMap(pos)) return -1; - -// Eigen::Vector3i id; -// posToIndex(pos, id); - -// return int(md_.occupancy_buffer_inflate_[toAddress(id)]); -// } - -// inline int GridMap::getOccupancy(Eigen::Vector3i id) { -// if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) || -// id(2) < 0 || id(2) >= mp_.map_voxel_num_(2)) -// return -1; - -// return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0; -// } - -// inline bool GridMap::isInMap(const Eigen::Vector3d& pos) { -// if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 || -// pos(2) < mp_.map_min_boundary_(2) + 1e-4) { -// // cout << "less than min range!" << endl; -// return false; -// } -// if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 || -// pos(2) > mp_.map_max_boundary_(2) - 1e-4) { -// return false; -// } -// return true; -// } - -// inline bool GridMap::isInMap(const Eigen::Vector3i& idx) { -// if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) { -// return false; -// } -// if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 || -// idx(2) > mp_.map_voxel_num_(2) - 1) { -// return false; -// } -// return true; -// } - -// inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) { -// for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_); -// } - -// inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) { -// for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i); -// } - -// inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector& pts) { -// int num = 0; - -// /* ---------- + shape inflate ---------- */ -// // for (int x = -step; x <= step; ++x) -// // { -// // if (x == 0) -// // continue; -// // pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2)); -// // } -// // for (int y = -step; y <= step; ++y) -// // { -// // if (y == 0) -// // continue; -// // pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2)); -// // } -// // for (int z = -1; z <= 1; ++z) -// // { -// // pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z); -// // } - -// /* ---------- all inflate ---------- */ -// for (int x = -step; x <= step; ++x) -// for (int y = -step; y <= step; ++y) -// for (int z = -step; z <= step; ++z) { -// pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z); -// } -// } - -// inline double GridMap::getResolution() { return mp_.resolution_; } - -// #endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/linear_obj_model.hpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/linear_obj_model.hpp deleted file mode 100644 index 574e3b00..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/linear_obj_model.hpp +++ /dev/null @@ -1,267 +0,0 @@ -/** -* This file is part of Fast-Planner. -* -* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, -* Developed by Boyu Zhou , -* for more information see . -* If you use this code, please cite the respective publications as -* listed on the above website. -* -* Fast-Planner is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* Fast-Planner is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with Fast-Planner. If not, see . -*/ - - - -#ifndef _LINEAR_OBJ_MODEL_H_ -#define _LINEAR_OBJ_MODEL_H_ - -#include -#include -#include - -class LinearObjModel { -private: - bool last_out_bound_{false}; - int input_type_; - /* data */ -public: - LinearObjModel(/* args */); - ~LinearObjModel(); - - void initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw, double yaw_dot, - Eigen::Vector3d color, Eigen::Vector3d scale, int input_type); - - void setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc); - - void update(double dt); // linear trippler integrator model - - static bool collide(LinearObjModel& obj1, LinearObjModel& obj2); - - // void setInput(Eigen::Vector3d acc) { - // acc_ = acc; - // } - void setInput(Eigen::Vector3d vel) { - vel_ = vel; - } - - void setYawDot(double yaw_dot) { - yaw_dot_ = yaw_dot; - } - - Eigen::Vector3d getPosition() { - return pos_; - } - void setPosition(Eigen::Vector3d pos) { - pos_ = pos; - } - - Eigen::Vector3d getVelocity() { - return vel_; - } - - void setVelocity(double x, double y, double z) { - vel_ = Eigen::Vector3d(x, y, z); - } - - Eigen::Vector3d getColor() { - return color_; - } - Eigen::Vector3d getScale() { - return scale_; - } - - double getYaw() { - return yaw_; - } - -private: - Eigen::Vector3d pos_, vel_, acc_; - Eigen::Vector3d color_, scale_; - double yaw_, yaw_dot_; - - Eigen::Vector3d bound_; - Eigen::Vector2d limit_v_, limit_a_; -}; - -LinearObjModel::LinearObjModel(/* args */) { -} - -LinearObjModel::~LinearObjModel() { -} - -void LinearObjModel::initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw, - double yaw_dot, Eigen::Vector3d color, Eigen::Vector3d scale, int input_type) { - pos_ = p; - vel_ = v; - acc_ = a; - color_ = color; - scale_ = scale; - input_type_ = input_type; - - yaw_ = yaw; - yaw_dot_ = yaw_dot; -} - -void LinearObjModel::setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc) { - bound_ = bound; - limit_v_ = vel; - limit_a_ = acc; -} - -void LinearObjModel::update(double dt) { - Eigen::Vector3d p0, v0, a0; - p0 = pos_, v0 = vel_, a0 = acc_; - //std::cout << v0.transpose() << std::endl; - - /* ---------- use acc as input ---------- */ - if ( input_type_ == 2 ) - { - vel_ = v0 + acc_ * dt; - for (int i = 0; i < 3; ++i) - { - if (vel_(i) > 0) vel_(i) = std::max(limit_v_(0), std::min(vel_(i), - limit_v_(1))); - if (vel_(i) <= 0) vel_(i) = std::max(-limit_v_(1), std::min(vel_(i), - -limit_v_(0))); - } - - pos_ = p0 + v0 * dt + 0.5 * acc_ * pow(dt, 2); - - /* ---------- reflect acc when collide with bound ---------- */ - if ( pos_(0) <= bound_(0) && pos_(0) >= -bound_(0) && - pos_(1) <= bound_(1) && pos_(1) >= -bound_(1) && - pos_(2) <= bound_(2) && pos_(2) >= 0 - ) - { - last_out_bound_ = false; - } - else if ( !last_out_bound_ ) - { - last_out_bound_ = true; - - // if ( pos_(0) > bound_(0) || pos_(0) < -bound_(0) ) acc_(0) = -acc_(0); - // if ( pos_(1) > bound_(1) || pos_(1) < -bound_(1) ) acc_(1) = -acc_(1); - // if ( pos_(2) > bound_(2) || pos_(2) < -bound_(2) ) acc_(2) = -acc_(2); - acc_ = -acc_; - //ROS_ERROR("AAAAAAAAAAAAAAAAAAa"); - } - } - // for (int i = 0; i < 2; ++i) - // { - // pos_(i) = std::min(pos_(i), bound_(i)); - // pos_(i) = std::max(pos_(i), -bound_(i)); - // } - // pos_(2) = std::min(pos_(2), bound_(2)); - // pos_(2) = std::max(pos_(2), 0.0); - - /* ---------- use vel as input ---------- */ - else if ( input_type_ == 1 ) - { - pos_ = p0 + v0 * dt; - for (int i = 0; i < 2; ++i) { - pos_(i) = std::min(pos_(i), bound_(i)); - pos_(i) = std::max(pos_(i), -bound_(i)); - } - pos_(2) = std::min(pos_(2), bound_(2)); - pos_(2) = std::max(pos_(2), 0.0); - - yaw_ += yaw_dot_ * dt; - - const double PI = 3.1415926; - if (yaw_ > 2 * PI) yaw_ -= 2 * PI; - - const double tol = 0.1; - if (pos_(0) > bound_(0) - tol) { - pos_(0) = bound_(0) - tol; - vel_(0) = -vel_(0); - } - if (pos_(0) < -bound_(0) + tol) { - pos_(0) = -bound_(0) + tol; - vel_(0) = -vel_(0); - } - - if (pos_(1) > bound_(1) - tol) { - pos_(1) = bound_(1) - tol; - vel_(1) = -vel_(1); - } - if (pos_(1) < -bound_(1) + tol) { - pos_(1) = -bound_(1) + tol; - vel_(1) = -vel_(1); - } - - if (pos_(2) > bound_(2) - tol) { - pos_(2) = bound_(2) - tol; - vel_(2) = -vel_(2); - } - if (pos_(2) < tol) { - pos_(2) = tol; - vel_(2) = -vel_(2); - } - } - - // /* ---------- reflect when collide with bound ---------- */ - - - //std::cout << pos_.transpose() << " " << bound_.transpose() << std::endl; -} - -bool LinearObjModel::collide(LinearObjModel& obj1, LinearObjModel& obj2) { - Eigen::Vector3d pos1, pos2, vel1, vel2, scale1, scale2; - pos1 = obj1.getPosition(); - vel1 = obj1.getVelocity(); - scale1 = obj1.getScale(); - - pos2 = obj2.getPosition(); - vel2 = obj2.getVelocity(); - scale2 = obj2.getScale(); - - /* ---------- collide ---------- */ - bool collide = fabs(pos1(0) - pos2(0)) < 0.5 * (scale1(0) + scale2(0)) && - fabs(pos1(1) - pos2(1)) < 0.5 * (scale1(1) + scale2(1)) && - fabs(pos1(2) - pos2(2)) < 0.5 * (scale1(2) + scale2(2)); - - if (collide) { - double tol[3]; - tol[0] = 0.5 * (scale1(0) + scale2(0)) - fabs(pos1(0) - pos2(0)); - tol[1] = 0.5 * (scale1(1) + scale2(1)) - fabs(pos1(1) - pos2(1)); - tol[2] = 0.5 * (scale1(2) + scale2(2)) - fabs(pos1(2) - pos2(2)); - - for (int i = 0; i < 3; ++i) { - if (tol[i] < tol[(i + 1) % 3] && tol[i] < tol[(i + 2) % 3]) { - vel1(i) = -vel1(i); - vel2(i) = -vel2(i); - obj1.setVelocity(vel1(0), vel1(1), vel1(2)); - obj2.setVelocity(vel2(0), vel2(1), vel2(2)); - - if (pos1(i) >= pos2(i)) { - pos1(i) += tol[i]; - pos2(i) -= tol[i]; - } else { - pos1(i) -= tol[i]; - pos2(i) += tol[i]; - } - obj1.setPosition(pos1); - obj2.setPosition(pos2); - - break; - } - } - - return true; - } else { - return false; - } -} - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/obj_predictor.h b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/obj_predictor.h deleted file mode 100644 index fb2e86bc..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/obj_predictor.h +++ /dev/null @@ -1,176 +0,0 @@ -/** -* This file is part of Fast-Planner. -* -* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, -* Developed by Boyu Zhou , -* for more information see . -* If you use this code, please cite the respective publications as -* listed on the above website. -* -* Fast-Planner is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* Fast-Planner is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with Fast-Planner. If not, see . -*/ - - - -#ifndef _OBJ_PREDICTOR_H_ -#define _OBJ_PREDICTOR_H_ - -#include -#include -#include -#include -#include -#include -#include - -using std::cout; -using std::endl; -using std::list; -using std::shared_ptr; -using std::unique_ptr; -using std::vector; - -namespace fast_planner { -class PolynomialPrediction; -typedef shared_ptr> ObjPrediction; -typedef shared_ptr> ObjScale; - -/* ========== prediction polynomial ========== */ -class PolynomialPrediction { -private: - vector> polys; - double t1, t2; // start / end - ros::Time global_start_time_; - -public: - PolynomialPrediction(/* args */) { - } - ~PolynomialPrediction() { - } - - void setPolynomial(vector>& pls) { - polys = pls; - } - void setTime(double t1, double t2) { - this->t1 = t1; - this->t2 = t2; - } - void setGlobalStartTime(ros::Time global_start_time) { - global_start_time_ = global_start_time; - } - - bool valid() { - return polys.size() == 3; - } - - /* note that t should be in [t1, t2] */ - Eigen::Vector3d evaluate(double t) { - Eigen::Matrix tv; - tv << 1.0, pow(t, 1), pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5); - - Eigen::Vector3d pt; - pt(0) = tv.dot(polys[0]), pt(1) = tv.dot(polys[1]), pt(2) = tv.dot(polys[2]); - - return pt; - } - - Eigen::Vector3d evaluateConstVel(double t) { - Eigen::Matrix tv; - tv << 1.0, pow(t-global_start_time_.toSec(), 1); - - // cout << t-global_start_time_.toSec() << endl; - - Eigen::Vector3d pt; - pt(0) = tv.dot(polys[0].head(2)), pt(1) = tv.dot(polys[1].head(2)), pt(2) = tv.dot(polys[2].head(2)); - - return pt; - } -}; - -/* ========== subscribe and record object history ========== */ -class ObjHistory { -public: - int skip_num_; - int queue_size_; - ros::Time global_start_time_; - - ObjHistory() { - } - ~ObjHistory() { - } - - void init(int id, int skip_num, int queue_size, ros::Time global_start_time); - - void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg); - - void clear() { - history_.clear(); - } - - void getHistory(list& his) { - his = history_; - } - -private: - list history_; // x,y,z;t - int skip_; - int obj_idx_; - Eigen::Vector3d scale_; -}; - -/* ========== predict future trajectory using history ========== */ -class ObjPredictor { -private: - ros::NodeHandle node_handle_; - - int obj_num_; - double lambda_; - double predict_rate_; - - vector pose_subs_; - ros::Subscriber marker_sub_; - ros::Timer predict_timer_; - vector> obj_histories_; - - /* share data with planner */ - ObjPrediction predict_trajs_; - ObjScale obj_scale_; - vector scale_init_; - - void markerCallback(const visualization_msgs::MarkerConstPtr& msg); - - void predictCallback(const ros::TimerEvent& e); - void predictPolyFit(); - void predictConstVel(); - -public: - ObjPredictor(/* args */); - ObjPredictor(ros::NodeHandle& node); - ~ObjPredictor(); - - void init(); - - ObjPrediction getPredictionTraj(); - ObjScale getObjScale(); - int getObjNums() {return obj_num_;} - - Eigen::Vector3d evaluatePoly(int obs_id, double time); - Eigen::Vector3d evaluateConstVel(int obs_id, double time); - - typedef shared_ptr Ptr; -}; - -} // namespace fast_planner - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/raycast.h b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/raycast.h deleted file mode 100644 index 23e08766..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/include/plan_env/raycast.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef RAYCAST_H_ -#define RAYCAST_H_ - -#include -#include - -double signum(double x); - -double mod(double value, double modulus); - -double intbound(double s, double ds); - -void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, - const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output); - -void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, - const Eigen::Vector3d& max, std::vector* output); - -class RayCaster { -private: - /* data */ - Eigen::Vector3d start_; - Eigen::Vector3d end_; - Eigen::Vector3d direction_; - Eigen::Vector3d min_; - Eigen::Vector3d max_; - int x_; - int y_; - int z_; - int endX_; - int endY_; - int endZ_; - double maxDist_; - double dx_; - double dy_; - double dz_; - int stepX_; - int stepY_; - int stepZ_; - double tMaxX_; - double tMaxY_; - double tMaxZ_; - double tDeltaX_; - double tDeltaY_; - double tDeltaZ_; - double dist_; - - int step_num_; - -public: - RayCaster(/* args */) { - } - ~RayCaster() { - } - - bool setInput(const Eigen::Vector3d& start, - const Eigen::Vector3d& end /* , const Eigen::Vector3d& min, - const Eigen::Vector3d& max */); - - bool step(Eigen::Vector3d& ray_pt); -}; - -#endif // RAYCAST_H_ \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/package.xml b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/package.xml deleted file mode 100644 index 7a55ca11..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - plan_env - 0.0.0 - The plan_env package - - - - - iszhouxin - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/grid_map.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/grid_map.cpp deleted file mode 100644 index c9ef59b2..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/grid_map.cpp +++ /dev/null @@ -1,1109 +0,0 @@ -#include "plan_env/grid_map.h" - -// #define current_img_ md_.depth_image_[image_cnt_ & 1] -// #define last_img_ md_.depth_image_[!(image_cnt_ & 1)] - -void GridMap::initMap(ros::NodeHandle &nh) -{ - node_ = nh; - - /* get parameter */ - int uav_id; - double x_size, y_size, z_size, x_origin, y_origin; - // 分辨率 - node_.param("grid_map/uav_id", uav_id, 1); - node_.param("grid_map/resolution", mp_.resolution_, -1.0); - node_.param("grid_map/map_size_x", x_size, -1.0); - node_.param("grid_map/map_size_y", y_size, -1.0); - node_.param("grid_map/map_size_z", z_size, -1.0); - node_.param("grid_map/map_origin_x", x_origin, -1.0); - node_.param("grid_map/map_origin_y", y_origin, -1.0); - node_.param("grid_map/local_update_range_x", mp_.local_update_range_(0), -1.0); - node_.param("grid_map/local_update_range_y", mp_.local_update_range_(1), -1.0); - node_.param("grid_map/local_update_range_z", mp_.local_update_range_(2), -1.0); - // 膨胀距离 - node_.param("grid_map/obstacles_inflation", mp_.obstacles_inflation_, -1.0); - // 相机参数 - node_.param("grid_map/fx", mp_.fx_, -1.0); - node_.param("grid_map/fy", mp_.fy_, -1.0); - node_.param("grid_map/cx", mp_.cx_, -1.0); - node_.param("grid_map/cy", mp_.cy_, -1.0); - // depth filter - node_.param("grid_map/use_depth_filter", mp_.use_depth_filter_, true); - node_.param("grid_map/depth_filter_tolerance", mp_.depth_filter_tolerance_, -1.0); - node_.param("grid_map/depth_filter_maxdist", mp_.depth_filter_maxdist_, -1.0); - node_.param("grid_map/depth_filter_mindist", mp_.depth_filter_mindist_, -1.0); - node_.param("grid_map/depth_filter_margin", mp_.depth_filter_margin_, -1); - node_.param("grid_map/k_depth_scaling_factor", mp_.k_depth_scaling_factor_, -1.0); - node_.param("grid_map/skip_pixel", mp_.skip_pixel_, -1); - // local fusion - node_.param("grid_map/p_hit", mp_.p_hit_, 0.70); - node_.param("grid_map/p_miss", mp_.p_miss_, 0.35); - node_.param("grid_map/p_min", mp_.p_min_, 0.12); - node_.param("grid_map/p_max", mp_.p_max_, 0.97); - node_.param("grid_map/p_occ", mp_.p_occ_, 0.80); - node_.param("grid_map/min_ray_length", mp_.min_ray_length_, -0.1); - node_.param("grid_map/max_ray_length", mp_.max_ray_length_, -0.1); - - node_.param("grid_map/visualization_truncate_height", mp_.visualization_truncate_height_, -0.1); - node_.param("grid_map/virtual_ceil_height", mp_.virtual_ceil_height_, -0.1); - node_.param("grid_map/virtual_ceil_yp", mp_.virtual_ceil_yp_, -0.1); - node_.param("grid_map/virtual_ceil_yn", mp_.virtual_ceil_yn_, -0.1); - - node_.param("grid_map/show_occ_time", mp_.show_occ_time_, false); - node_.param("grid_map/pose_type", mp_.pose_type_, 1); - - node_.param("grid_map/frame_id", mp_.frame_id_, string("world")); - // 地图边缘 - node_.param("grid_map/local_map_margin", mp_.local_map_margin_, 1); - // 地面高度 - node_.param("grid_map/ground_height", mp_.ground_height_, 0.0); - - node_.param("grid_map/odom_depth_timeout", mp_.odom_depth_timeout_, 1.0); - - if( mp_.virtual_ceil_height_ - mp_.ground_height_ > z_size) - { - // 天花板高度 = 地面高度 + z_size - mp_.virtual_ceil_height_ = mp_.ground_height_ + z_size; - } - - mp_.resolution_inv_ = 1 / mp_.resolution_; - //todo: different map origin - if(x_origin < -1.0+1e-2 && x_origin > -1.0-1e-2) mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_); - else mp_.map_origin_ = Eigen::Vector3d(x_origin, y_origin, mp_.ground_height_); - mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size); - - //logit(x) (log((x) / (1 - (x)))) - mp_.prob_hit_log_ = logit(mp_.p_hit_); - mp_.prob_miss_log_ = logit(mp_.p_miss_); - mp_.clamp_min_log_ = logit(mp_.p_min_); - mp_.clamp_max_log_ = logit(mp_.p_max_); - mp_.min_occupancy_log_ = logit(mp_.p_occ_); - mp_.unknown_flag_ = 0.01; - - // cout << "hit: " << mp_.prob_hit_log_ << endl; - // cout << "miss: " << mp_.prob_miss_log_ << endl; - // cout << "min log: " << mp_.clamp_min_log_ << endl; - // cout << "max: " << mp_.clamp_max_log_ << endl; - // cout << "thresh log: " << mp_.min_occupancy_log_ << endl; - - for (int i = 0; i < 3; ++i) - mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_); - - mp_.map_min_boundary_ = mp_.map_origin_; - mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_; - - // initialize data buffers - - int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2); - - md_.occupancy_buffer_ = vector(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_); - md_.occupancy_buffer_inflate_ = vector(buffer_size, 0); - - md_.count_hit_and_miss_ = vector(buffer_size, 0); - md_.count_hit_ = vector(buffer_size, 0); - md_.flag_rayend_ = vector(buffer_size, -1); - md_.flag_traverse_ = vector(buffer_size, -1); - - md_.raycast_num_ = 0; - - md_.proj_points_.resize(640 * 480 / mp_.skip_pixel_ / mp_.skip_pixel_); - md_.proj_points_cnt = 0; - - // 相机外参数 - md_.cam2body_ << 0.0, 0.0, 1.0, 0.0, - -1.0, 0.0, 0.0, 0.0, - 0.0, -1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0; - - /* init callback */ - - depth_sub_.reset(new message_filters::Subscriber(node_, "grid_map/depth", 50)); - // 相机外参 - extrinsic_sub_ = node_.subscribe("/vins_estimator/extrinsic", 10, &GridMap::extrinsicCallback, this); //sub - - if (mp_.pose_type_ == POSE_STAMPED) - { - pose_sub_.reset( - new message_filters::Subscriber(node_, "grid_map/pose", 25)); - - sync_image_pose_.reset(new message_filters::Synchronizer( - SyncPolicyImagePose(100), *depth_sub_, *pose_sub_)); - sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2)); - } - else if (mp_.pose_type_ == ODOMETRY) - { - odom_sub_.reset(new message_filters::Subscriber(node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay())); - - sync_image_odom_.reset(new message_filters::Synchronizer( - SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_)); - sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2)); - } - - // use odometry and point cloud - indep_cloud_sub_ = - node_.subscribe("grid_map/cloud", 10, &GridMap::cloudCallback, this); - indep_odom_sub_ = - node_.subscribe("grid_map/odom", 10, &GridMap::odomCallback, this); - - occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this); - vis_timer_ = node_.createTimer(ros::Duration(0.11), &GridMap::visCallback, this); - - map_pub_ = node_.advertise("grid_map/occupancy", 10); - map_inf_pub_ = node_.advertise("grid_map/occupancy_inflate", 10); - - md_.occ_need_update_ = false; - md_.local_updated_ = false; - md_.has_first_depth_ = false; - md_.has_odom_ = false; - md_.has_cloud_ = false; - md_.image_cnt_ = 0; - md_.last_occ_update_time_.fromSec(0); - - md_.fuse_time_ = 0.0; - md_.update_num_ = 0; - md_.max_fuse_time_ = 0.0; - - md_.flag_depth_odom_timeout_ = false; - md_.flag_use_depth_fusion = false; - - // rand_noise_ = uniform_real_distribution(-0.2, 0.2); - // rand_noise2_ = normal_distribution(0, 0.2); - // random_device rd; - // eng_ = default_random_engine(rd()); -} - -void GridMap::resetBuffer() -{ - Eigen::Vector3d min_pos = mp_.map_min_boundary_; - Eigen::Vector3d max_pos = mp_.map_max_boundary_; - - resetBuffer(min_pos, max_pos); - - md_.local_bound_min_ = Eigen::Vector3i::Zero(); - md_.local_bound_max_ = mp_.map_voxel_num_ - Eigen::Vector3i::Ones(); -} - -void GridMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos) -{ - - Eigen::Vector3i min_id, max_id; - - /* reset occ and dist buffer */ - for (int x = min_id(0); x <= max_id(0); ++x) - for (int y = min_id(1); y <= max_id(1); ++y) - for (int z = min_id(2); z <= max_id(2); ++z) - { - md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0; - } -} - -int GridMap::setCacheOccupancy(Eigen::Vector3d pos, int occ) -{ - if (occ != 1 && occ != 0) - return INVALID_IDX; - - Eigen::Vector3i id; - posToIndex(pos, id); - int idx_ctns = toAddress(id); - - md_.count_hit_and_miss_[idx_ctns] += 1; - - if (md_.count_hit_and_miss_[idx_ctns] == 1) - { - md_.cache_voxel_.push(id); - } - - if (occ == 1) - md_.count_hit_[idx_ctns] += 1; - - return idx_ctns; -} - -void GridMap::projectDepthImage() -{ - // md_.proj_points_.clear(); - md_.proj_points_cnt = 0; - - uint16_t *row_ptr; - // int cols = current_img_.cols, rows = current_img_.rows; - int cols = md_.depth_image_.cols; - int rows = md_.depth_image_.rows; - int skip_pix = mp_.skip_pixel_; - - double depth; - - Eigen::Matrix3d camera_r = md_.camera_r_m_; - - if (!mp_.use_depth_filter_) - { - for (int v = 0; v < rows; v+=skip_pix) - { - row_ptr = md_.depth_image_.ptr(v); - - for (int u = 0; u < cols; u+=skip_pix) - { - - Eigen::Vector3d proj_pt; - depth = (*row_ptr++) / mp_.k_depth_scaling_factor_; - proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_; - proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_; - proj_pt(2) = depth; - - proj_pt = camera_r * proj_pt + md_.camera_pos_; - - if (u == 320 && v == 240) - std::cout << "depth: " << depth << std::endl; - md_.proj_points_[md_.proj_points_cnt++] = proj_pt; - } - } - } - /* use depth filter */ - else - { - - if (!md_.has_first_depth_) - md_.has_first_depth_ = true; - else - { - Eigen::Vector3d pt_cur, pt_world, pt_reproj; - - Eigen::Matrix3d last_camera_r_inv; - last_camera_r_inv = md_.last_camera_r_m_.inverse(); - const double inv_factor = 1.0 / mp_.k_depth_scaling_factor_; - - for (int v = mp_.depth_filter_margin_; v < rows - mp_.depth_filter_margin_; v += mp_.skip_pixel_) - { - row_ptr = md_.depth_image_.ptr(v) + mp_.depth_filter_margin_; - - for (int u = mp_.depth_filter_margin_; u < cols - mp_.depth_filter_margin_; - u += mp_.skip_pixel_) - { - - depth = (*row_ptr) * inv_factor; - row_ptr = row_ptr + mp_.skip_pixel_; - - // filter depth - // depth += rand_noise_(eng_); - // if (depth > 0.01) depth += rand_noise2_(eng_); - - if (*row_ptr == 0) - { - depth = mp_.max_ray_length_ + 0.1; - } - else if (depth < mp_.depth_filter_mindist_) - { - continue; - } - else if (depth > mp_.depth_filter_maxdist_) - { - depth = mp_.max_ray_length_ + 0.1; - } - - // project to world frame - pt_cur(0) = (u - mp_.cx_) * depth / mp_.fx_; - pt_cur(1) = (v - mp_.cy_) * depth / mp_.fy_; - pt_cur(2) = depth; - - pt_world = camera_r * pt_cur + md_.camera_pos_; - // if (!isInMap(pt_world)) { - // pt_world = closetPointInMap(pt_world, md_.camera_pos_); - // } - - md_.proj_points_[md_.proj_points_cnt++] = pt_world; - - // check consistency with last image, disabled... - if (false) - { - pt_reproj = last_camera_r_inv * (pt_world - md_.last_camera_pos_); - double uu = pt_reproj.x() * mp_.fx_ / pt_reproj.z() + mp_.cx_; - double vv = pt_reproj.y() * mp_.fy_ / pt_reproj.z() + mp_.cy_; - - if (uu >= 0 && uu < cols && vv >= 0 && vv < rows) - { - if (fabs(md_.last_depth_image_.at((int)vv, (int)uu) * inv_factor - - pt_reproj.z()) < mp_.depth_filter_tolerance_) - { - md_.proj_points_[md_.proj_points_cnt++] = pt_world; - } - } - else - { - md_.proj_points_[md_.proj_points_cnt++] = pt_world; - } - } - } - } - } - } - - /* maintain camera pose for consistency check */ - - md_.last_camera_pos_ = md_.camera_pos_; - md_.last_camera_r_m_ = md_.camera_r_m_; - md_.last_depth_image_ = md_.depth_image_; -} - -void GridMap::raycastProcess() -{ - // if (md_.proj_points_.size() == 0) - if (md_.proj_points_cnt == 0) - return; - - ros::Time t1, t2; - - md_.raycast_num_ += 1; - - int vox_idx; - double length; - - // bounding box of updated region - double min_x = mp_.map_max_boundary_(0); - double min_y = mp_.map_max_boundary_(1); - double min_z = mp_.map_max_boundary_(2); - // printf("bounding box min:[%f,%f,%f]\n",min_x,min_y,min_z); - - double max_x = mp_.map_min_boundary_(0); - double max_y = mp_.map_min_boundary_(1); - double max_z = mp_.map_min_boundary_(2); - // printf("bounding box max:[%f,%f,%f]\n",max_x,max_y,max_z); - - RayCaster raycaster; - Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5); - Eigen::Vector3d ray_pt, pt_w; - - for (int i = 0; i < md_.proj_points_cnt; ++i) - { - pt_w = md_.proj_points_[i]; - - // set flag for projected point - - if (!isInMap(pt_w)) - { - pt_w = closetPointInMap(pt_w, md_.camera_pos_); - - length = (pt_w - md_.camera_pos_).norm(); - if (length > mp_.max_ray_length_) - { - pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_; - } - vox_idx = setCacheOccupancy(pt_w, 0); - } - else - { - length = (pt_w - md_.camera_pos_).norm(); - - if (length > mp_.max_ray_length_) - { - pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_; - vox_idx = setCacheOccupancy(pt_w, 0); - } - else - { - vox_idx = setCacheOccupancy(pt_w, 1); - } - } - - max_x = max(max_x, pt_w(0)); - max_y = max(max_y, pt_w(1)); - max_z = max(max_z, pt_w(2)); - - min_x = min(min_x, pt_w(0)); - min_y = min(min_y, pt_w(1)); - min_z = min(min_z, pt_w(2)); - - // raycasting between camera center and point - - if (vox_idx != INVALID_IDX) - { - if (md_.flag_rayend_[vox_idx] == md_.raycast_num_) - { - continue; - } - else - { - md_.flag_rayend_[vox_idx] = md_.raycast_num_; - } - } - - raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_); - - while (raycaster.step(ray_pt)) - { - Eigen::Vector3d tmp = (ray_pt + half) * mp_.resolution_; - length = (tmp - md_.camera_pos_).norm(); - - // if (length < mp_.min_ray_length_) break; - - vox_idx = setCacheOccupancy(tmp, 0); - - if (vox_idx != INVALID_IDX) - { - if (md_.flag_traverse_[vox_idx] == md_.raycast_num_) - { - break; - } - else - { - md_.flag_traverse_[vox_idx] = md_.raycast_num_; - } - } - } - } - - min_x = min(min_x, md_.camera_pos_(0)); - min_y = min(min_y, md_.camera_pos_(1)); - min_z = min(min_z, md_.camera_pos_(2)); - - max_x = max(max_x, md_.camera_pos_(0)); - max_y = max(max_y, md_.camera_pos_(1)); - max_z = max(max_z, md_.camera_pos_(2)); - max_z = max(max_z, mp_.ground_height_); - - // printf("bounding box max:000[%f,%f,%f]\n",max_x,max_y,max_z); - // printf("bounding box min:000[%f,%f,%f]\n",min_x,min_x,min_x); - posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_); - posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_); - // printf("md_.local_bound_min_000:[%d,%d,%d]\n",md_.local_bound_min_(0),md_.local_bound_min_(1),md_.local_bound_min_(2)); - boundIndex(md_.local_bound_min_); - // printf("md_.local_bound_max_000:[%d,%d,%d]\n",md_.local_bound_max_(0),md_.local_bound_max_(1),md_.local_bound_max_(2)); - boundIndex(md_.local_bound_max_); - - md_.local_updated_ = true; - - // update occupancy cached in queue - Eigen::Vector3d local_range_min = md_.camera_pos_ - mp_.local_update_range_; - Eigen::Vector3d local_range_max = md_.camera_pos_ + mp_.local_update_range_; - - Eigen::Vector3i min_id, max_id; - posToIndex(local_range_min, min_id); - posToIndex(local_range_max, max_id); - boundIndex(min_id); - boundIndex(max_id); - - // std::cout << "cache all: " << md_.cache_voxel_.size() << std::endl; - - while (!md_.cache_voxel_.empty()) - { - - Eigen::Vector3i idx = md_.cache_voxel_.front(); - int idx_ctns = toAddress(idx); - md_.cache_voxel_.pop(); - - double log_odds_update = - md_.count_hit_[idx_ctns] >= md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_; - - md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0; - - if (log_odds_update >= 0 && md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_) - { - continue; - } - else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_) - { - md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_; - continue; - } - - bool in_local = idx(0) >= min_id(0) && idx(0) <= max_id(0) && idx(1) >= min_id(1) && - idx(1) <= max_id(1) && idx(2) >= min_id(2) && idx(2) <= max_id(2); - if (!in_local) - { - md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_; - } - - md_.occupancy_buffer_[idx_ctns] = - std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_), - mp_.clamp_max_log_); - } -} - -Eigen::Vector3d GridMap::closetPointInMap(const Eigen::Vector3d &pt, const Eigen::Vector3d &camera_pt) -{ - Eigen::Vector3d diff = pt - camera_pt; - Eigen::Vector3d max_tc = mp_.map_max_boundary_ - camera_pt; - Eigen::Vector3d min_tc = mp_.map_min_boundary_ - camera_pt; - - double min_t = 1000000; - - for (int i = 0; i < 3; ++i) - { - if (fabs(diff[i]) > 0) - { - - double t1 = max_tc[i] / diff[i]; - if (t1 > 0 && t1 < min_t) - min_t = t1; - - double t2 = min_tc[i] / diff[i]; - if (t2 > 0 && t2 < min_t) - min_t = t2; - } - } - - return camera_pt + (min_t - 1e-3) * diff; -} - -void GridMap::clearAndInflateLocalMap() -{ - /*clear outside local*/ - const int vec_margin = 5; - // Eigen::Vector3i min_vec_margin = min_vec - Eigen::Vector3i(vec_margin, - // vec_margin, vec_margin); Eigen::Vector3i max_vec_margin = max_vec + - // Eigen::Vector3i(vec_margin, vec_margin, vec_margin); - - // printf("md_.local_bound_min_:[%d,%d,%d]\n",md_.local_bound_min_(0),md_.local_bound_min_(1),md_.local_bound_min_(2)); - Eigen::Vector3i min_cut = md_.local_bound_min_ - - Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_); - // printf("md_.local_bound_max_:[%d,%d,%d]\n",md_.local_bound_max_(0),md_.local_bound_max_(1),md_.local_bound_max_(2)); - Eigen::Vector3i max_cut = md_.local_bound_max_ + - Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_); - boundIndex(min_cut); - // printf("min_cut:[%d,%d,%d]\n",min_cut(0),min_cut(1),min_cut(2)); - boundIndex(max_cut); - // printf("max_cut:[%d,%d,%d]\n",max_cut(0),max_cut(1),max_cut(2)); - - Eigen::Vector3i min_cut_m = min_cut - Eigen::Vector3i(vec_margin, vec_margin, vec_margin); - Eigen::Vector3i max_cut_m = max_cut + Eigen::Vector3i(vec_margin, vec_margin, vec_margin); - boundIndex(min_cut_m); - // printf("min_cut_m:[%d,%d,%d]\n",min_cut_m(0),min_cut_m(1),min_cut_m(2)); - boundIndex(max_cut_m); - // printf("max_cut_m:[%d,%d,%d]\n\n",max_cut_m(0),max_cut_m(1),max_cut_m(2)); - - // clear data outside the local range - - for (int x = min_cut_m(0); x <= max_cut_m(0); ++x) - for (int y = min_cut_m(1); y <= max_cut_m(1); ++y) - { - - for (int z = min_cut_m(2); z < min_cut(2); ++z) - { - int idx = toAddress(x, y, z); - md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; - } - - for (int z = max_cut(2) + 1; z <= max_cut_m(2); ++z) - { - int idx = toAddress(x, y, z); - md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; - } - } - - for (int z = min_cut_m(2); z <= max_cut_m(2); ++z) - for (int x = min_cut_m(0); x <= max_cut_m(0); ++x) - { - - for (int y = min_cut_m(1); y < min_cut(1); ++y) - { - int idx = toAddress(x, y, z); - md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; - } - - for (int y = max_cut(1) + 1; y <= max_cut_m(1); ++y) - { - int idx = toAddress(x, y, z); - md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; - } - } - - for (int y = min_cut_m(1); y <= max_cut_m(1); ++y) - for (int z = min_cut_m(2); z <= max_cut_m(2); ++z) - { - - for (int x = min_cut_m(0); x < min_cut(0); ++x) - { - int idx = toAddress(x, y, z); - md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; - } - - for (int x = max_cut(0) + 1; x <= max_cut_m(0); ++x) - { - int idx = toAddress(x, y, z); - md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; - } - } - - // inflate occupied voxels to compensate robot size - - int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_); - // int inf_step_z = 1; - vector inf_pts(pow(2 * inf_step + 1, 3)); - // inf_pts.resize(4 * inf_step + 3); - Eigen::Vector3i inf_pt; - - // clear outdated data - for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) - for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) - for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z) - { - md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0; - } - - // inflate obstacles - for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) - for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) - for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z) - { - - if (md_.occupancy_buffer_[toAddress(x, y, z)] > mp_.min_occupancy_log_) - { - inflatePoint(Eigen::Vector3i(x, y, z), inf_step, inf_pts); - - for (int k = 0; k < (int)inf_pts.size(); ++k) - { - inf_pt = inf_pts[k]; - int idx_inf = toAddress(inf_pt); - if (idx_inf < 0 || - idx_inf >= mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2)) - { - continue; - } - md_.occupancy_buffer_inflate_[idx_inf] = 1; - } - } - } - - // add virtual ceiling to limit flight height - if (mp_.virtual_ceil_height_ > -0.5) { - int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_) - 1; - for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) - for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) { - md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1; - } - } -} - -void GridMap::visCallback(const ros::TimerEvent & /*event*/) -{ - - publishMapInflate(true); - publishMap(); -} - -void GridMap::updateOccupancyCallback(const ros::TimerEvent & /*event*/) -{ - if (md_.last_occ_update_time_.toSec() < 1.0 ) md_.last_occ_update_time_ = ros::Time::now(); - - if (!md_.occ_need_update_) - { - if ( md_.flag_use_depth_fusion && (ros::Time::now() - md_.last_occ_update_time_).toSec() > mp_.odom_depth_timeout_ ) - { - ROS_ERROR("odom or depth lost! ros::Time::now()=%f, md_.last_occ_update_time_=%f, mp_.odom_depth_timeout_=%f", - ros::Time::now().toSec(), md_.last_occ_update_time_.toSec(), mp_.odom_depth_timeout_); - md_.flag_depth_odom_timeout_ = true; - } - return; - } - - // std::cout << "updateOccupancyCallback 1!" << std::endl; - md_.last_occ_update_time_ = ros::Time::now(); - - /* update occupancy */ - // ros::Time t1, t2, t3, t4; - // t1 = ros::Time::now(); - - projectDepthImage(); - // t2 = ros::Time::now(); - raycastProcess(); - // t3 = ros::Time::now(); - - if (md_.local_updated_) - clearAndInflateLocalMap(); - - // t4 = ros::Time::now(); - - // cout << setprecision(7); - // cout << "t2=" << (t2-t1).toSec() << " t3=" << (t3-t2).toSec() << " t4=" << (t4-t3).toSec() << endl;; - - // md_.fuse_time_ += (t2 - t1).toSec(); - // md_.max_fuse_time_ = max(md_.max_fuse_time_, (t2 - t1).toSec()); - - // if (mp_.show_occ_time_) - // ROS_WARN("Fusion: cur t = %lf, avg t = %lf, max t = %lf", (t2 - t1).toSec(), - // md_.fuse_time_ / md_.update_num_, md_.max_fuse_time_); - - md_.occ_need_update_ = false; - md_.local_updated_ = false; -} - -void GridMap::depthPoseCallback(const sensor_msgs::ImageConstPtr &img, - const geometry_msgs::PoseStampedConstPtr &pose) -{ - - std::cout << "test 1!" << std::endl; - - /* get depth image */ - cv_bridge::CvImagePtr cv_ptr; - cv_ptr = cv_bridge::toCvCopy(img, img->encoding); - - if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1) - { - (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_); - } - cv_ptr->image.copyTo(md_.depth_image_); - - // std::cout << "depth: " << md_.depth_image_.cols << ", " << md_.depth_image_.rows << std::endl; - - /* get pose */ - md_.camera_pos_(0) = pose->pose.position.x; - md_.camera_pos_(1) = pose->pose.position.y; - md_.camera_pos_(2) = pose->pose.position.z; - md_.camera_r_m_ = Eigen::Quaterniond(pose->pose.orientation.w, pose->pose.orientation.x, - pose->pose.orientation.y, pose->pose.orientation.z) - .toRotationMatrix(); - if (isInMap(md_.camera_pos_)) - { - md_.has_odom_ = true; - md_.update_num_ += 1; - md_.occ_need_update_ = true; - } - else - { - md_.occ_need_update_ = false; - } - - md_.flag_use_depth_fusion = true; -} - -void GridMap::odomCallback(const nav_msgs::OdometryConstPtr &odom) -{ - if (md_.has_first_depth_) - return; - - md_.camera_pos_(0) = odom->pose.pose.position.x; - md_.camera_pos_(1) = odom->pose.pose.position.y; - md_.camera_pos_(2) = odom->pose.pose.position.z; - - md_.has_odom_ = true; -} - -void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img) -{ - - pcl::PointCloud latest_cloud; - pcl::fromROSMsg(*img, latest_cloud); - - md_.has_cloud_ = true; - - if (!md_.has_odom_) - { - std::cout << "no odom!" << std::endl; - return; - } - - //ROS_INFO("test 3"); - - if (latest_cloud.points.size() == 0) - return; - - if (isnan(md_.camera_pos_(0)) || isnan(md_.camera_pos_(1)) || isnan(md_.camera_pos_(2))) - return; - - this->resetBuffer(md_.camera_pos_ - mp_.local_update_range_, - md_.camera_pos_ + mp_.local_update_range_); - - pcl::PointXYZ pt; - Eigen::Vector3d p3d, p3d_inf; - - int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_); - int inf_step_z = 1; - - double max_x, max_y, max_z, min_x, min_y, min_z; - - min_x = mp_.map_max_boundary_(0); - min_y = mp_.map_max_boundary_(1); - min_z = mp_.map_max_boundary_(2); - - max_x = mp_.map_min_boundary_(0); - max_y = mp_.map_min_boundary_(1); - max_z = mp_.map_min_boundary_(2); - - for (size_t i = 0; i < latest_cloud.points.size(); ++i) - { - pt = latest_cloud.points[i]; - p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z; - - /* point inside update range */ - Eigen::Vector3d devi = p3d - md_.camera_pos_; - Eigen::Vector3i inf_pt; - - if (fabs(devi(0)) < mp_.local_update_range_(0) && fabs(devi(1)) < mp_.local_update_range_(1) && - fabs(devi(2)) < mp_.local_update_range_(2)) - { - - /* inflate the point */ - for (int x = -inf_step; x <= inf_step; ++x) - for (int y = -inf_step; y <= inf_step; ++y) - for (int z = -inf_step_z; z <= inf_step_z; ++z) - { - - p3d_inf(0) = pt.x + x * mp_.resolution_; - p3d_inf(1) = pt.y + y * mp_.resolution_; - p3d_inf(2) = pt.z + z * mp_.resolution_; - - max_x = max(max_x, p3d_inf(0)); - max_y = max(max_y, p3d_inf(1)); - max_z = max(max_z, p3d_inf(2)); - - min_x = min(min_x, p3d_inf(0)); - min_y = min(min_y, p3d_inf(1)); - min_z = min(min_z, p3d_inf(2)); - - posToIndex(p3d_inf, inf_pt); - - if (!isInMap(inf_pt)) - continue; - - int idx_inf = toAddress(inf_pt); - - md_.occupancy_buffer_inflate_[idx_inf] = 1; - } - } - } - - min_x = min(min_x, md_.camera_pos_(0)); - min_y = min(min_y, md_.camera_pos_(1)); - min_z = min(min_z, md_.camera_pos_(2)); - - max_x = max(max_x, md_.camera_pos_(0)); - max_y = max(max_y, md_.camera_pos_(1)); - max_z = max(max_z, md_.camera_pos_(2)); - - max_z = max(max_z, mp_.ground_height_); - - posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_); - posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_); - - boundIndex(md_.local_bound_min_); - boundIndex(md_.local_bound_max_); - - // add virtual ceiling to limit flight height - if (mp_.virtual_ceil_height_ > -0.5) { - int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_) - 1; - for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) - for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) { - md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1; - } - } -} - -void GridMap::publishMap() -{ - - if (map_pub_.getNumSubscribers() <= 0) - return; - - pcl::PointXYZ pt; - pcl::PointCloud cloud; - - Eigen::Vector3i min_cut = md_.local_bound_min_; - Eigen::Vector3i max_cut = md_.local_bound_max_; - - int lmm = mp_.local_map_margin_ / 2; - min_cut -= Eigen::Vector3i(lmm, lmm, lmm); - max_cut += Eigen::Vector3i(lmm, lmm, lmm); - - boundIndex(min_cut); - boundIndex(max_cut); - - for (int x = min_cut(0); x <= max_cut(0); ++x) - for (int y = min_cut(1); y <= max_cut(1); ++y) - for (int z = min_cut(2); z <= max_cut(2); ++z) - { - if (md_.occupancy_buffer_[toAddress(x, y, z)] < mp_.min_occupancy_log_) - continue; - - Eigen::Vector3d pos; - indexToPos(Eigen::Vector3i(x, y, z), pos); - if (pos(2) > mp_.visualization_truncate_height_) - continue; - - pt.x = pos(0); - pt.y = pos(1); - pt.z = pos(2); - cloud.push_back(pt); - } - - //border - for(int i = 0; i < mp_.map_voxel_num_(0); i++) - { - pt.x = mp_.map_min_boundary_(0)+i*mp_.resolution_; - pt.y = mp_.map_min_boundary_(1); - pt.z = mp_.map_min_boundary_(2); - cloud.push_back(pt); - - pt.x = mp_.map_min_boundary_(0)+i*mp_.resolution_; - pt.y = mp_.map_max_boundary_(1); - pt.z = mp_.map_min_boundary_(2); - cloud.push_back(pt); - } - - for(int i = 0; i < mp_.map_voxel_num_(1); i++) - { - pt.x = mp_.map_min_boundary_(0); - pt.y = mp_.map_min_boundary_(1)+i*mp_.resolution_; - pt.z = mp_.map_min_boundary_(2); - cloud.push_back(pt); - - pt.x = mp_.map_max_boundary_(0); - pt.y = mp_.map_min_boundary_(1)+i*mp_.resolution_; - pt.z = mp_.map_min_boundary_(2); - cloud.push_back(pt); - } - - cloud.width = cloud.points.size(); - cloud.height = 1; - cloud.is_dense = true; - cloud.header.frame_id = mp_.frame_id_; - - sensor_msgs::PointCloud2 cloud_msg; - - pcl::toROSMsg(cloud, cloud_msg); - map_pub_.publish(cloud_msg); -} - -void GridMap::publishMapInflate(bool all_info) -{ - - if (map_inf_pub_.getNumSubscribers() <= 0) - return; - - pcl::PointXYZ pt; - pcl::PointCloud cloud; - - Eigen::Vector3i min_cut = md_.local_bound_min_; - Eigen::Vector3i max_cut = md_.local_bound_max_; - - if (all_info) - { - int lmm = mp_.local_map_margin_; - min_cut -= Eigen::Vector3i(lmm, lmm, lmm); - max_cut += Eigen::Vector3i(lmm, lmm, lmm); - } - - boundIndex(min_cut); - boundIndex(max_cut); - - for (int x = min_cut(0); x <= max_cut(0); ++x) - for (int y = min_cut(1); y <= max_cut(1); ++y) - for (int z = min_cut(2); z <= max_cut(2); ++z) - { - if (md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 0) - continue; - - Eigen::Vector3d pos; - indexToPos(Eigen::Vector3i(x, y, z), pos); - if (pos(2) > mp_.visualization_truncate_height_) - continue; - - pt.x = pos(0); - pt.y = pos(1); - pt.z = pos(2); - cloud.push_back(pt); - } - - //border x - for(int i = 0; i < mp_.map_voxel_num_(0); i++) - { - pt.x = mp_.map_min_boundary_(0)+i*mp_.resolution_; - pt.y = mp_.map_min_boundary_(1); - pt.z = mp_.map_min_boundary_(2); - cloud.push_back(pt); - - pt.x = mp_.map_min_boundary_(0)+i*mp_.resolution_; - pt.y = mp_.map_max_boundary_(1); - pt.z = mp_.map_min_boundary_(2); - cloud.push_back(pt); - } - //border y - for(int i = 0; i < mp_.map_voxel_num_(1); i++) - { - pt.x = mp_.map_min_boundary_(0); - pt.y = mp_.map_min_boundary_(1)+i*mp_.resolution_; - pt.z = mp_.map_min_boundary_(2); - cloud.push_back(pt); - - pt.x = mp_.map_max_boundary_(0); - pt.y = mp_.map_min_boundary_(1)+i*mp_.resolution_; - pt.z = mp_.map_min_boundary_(2); - cloud.push_back(pt); - } - - cloud.width = cloud.points.size(); - cloud.height = 1; - cloud.is_dense = true; - cloud.header.frame_id = mp_.frame_id_; - sensor_msgs::PointCloud2 cloud_msg; - - pcl::toROSMsg(cloud, cloud_msg); - map_inf_pub_.publish(cloud_msg); - - // ROS_INFO("pub map"); -} - -bool GridMap::odomValid() { return md_.has_odom_; } - -bool GridMap::hasDepthObservation() { return md_.has_first_depth_; } - -Eigen::Vector3d GridMap::getOrigin() { return mp_.map_origin_; } - -// int GridMap::getVoxelNum() { -// return mp_.map_voxel_num_[0] * mp_.map_voxel_num_[1] * mp_.map_voxel_num_[2]; -// } - -void GridMap::getRegion(Eigen::Vector3d &ori, Eigen::Vector3d &size) -{ - ori = mp_.map_origin_, size = mp_.map_size_; -} - -void GridMap::extrinsicCallback(const nav_msgs::OdometryConstPtr &odom) -{ - Eigen::Quaterniond cam2body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, - odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, - odom->pose.pose.orientation.z); - Eigen::Matrix3d cam2body_r_m = cam2body_q.toRotationMatrix(); - md_.cam2body_.block<3, 3>(0, 0) = cam2body_r_m; - md_.cam2body_(0, 3) = odom->pose.pose.position.x; - md_.cam2body_(1, 3) = odom->pose.pose.position.y; - md_.cam2body_(2, 3) = odom->pose.pose.position.z; - md_.cam2body_(3, 3) = 1.0; -} - -void GridMap::depthOdomCallback(const sensor_msgs::ImageConstPtr &img, - const nav_msgs::OdometryConstPtr &odom) -{ - /* get pose */ - Eigen::Quaterniond body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, - odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, - odom->pose.pose.orientation.z); - Eigen::Matrix3d body_r_m = body_q.toRotationMatrix(); - Eigen::Matrix4d body2world; - body2world.block<3, 3>(0, 0) = body_r_m; - body2world(0, 3) = odom->pose.pose.position.x; - body2world(1, 3) = odom->pose.pose.position.y; - body2world(2, 3) = odom->pose.pose.position.z; - body2world(3, 3) = 1.0; - - Eigen::Matrix4d cam_T = body2world * md_.cam2body_; - md_.camera_pos_(0) = cam_T(0, 3); - md_.camera_pos_(1) = cam_T(1, 3); - md_.camera_pos_(2) = cam_T(2, 3); - md_.camera_r_m_ = cam_T.block<3, 3>(0, 0); - - /* get depth image */ - cv_bridge::CvImagePtr cv_ptr; - cv_ptr = cv_bridge::toCvCopy(img, img->encoding); - if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1) - { - (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_); - } - cv_ptr->image.copyTo(md_.depth_image_); - - md_.occ_need_update_ = true; - md_.flag_use_depth_fusion = true; - - // reset depth lost flag - if(md_.flag_depth_odom_timeout_ == true) md_.flag_depth_odom_timeout_ = false; -} diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/obj_generator.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/obj_generator.cpp deleted file mode 100644 index 20b07011..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/obj_generator.cpp +++ /dev/null @@ -1,238 +0,0 @@ -/** -* This file is part of Fast-Planner. -* -* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, -* Developed by Boyu Zhou , -* for more information see . -* If you use this code, please cite the respective publications as -* listed on the above website. -* -* Fast-Planner is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* Fast-Planner is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with Fast-Planner. If not, see . -*/ - - - -#include "visualization_msgs/Marker.h" -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -using namespace std; - -int obj_num, _input_type; -double _x_size, _y_size, _h_size, _vel, _yaw_dot, _acc_r1, _acc_r2, _acc_z, _scale1, _scale2, _interval; - - -ros::Publisher obj_pub; // visualize marker -vector pose_pubs; // obj pose (from optitrack) -vector obj_models; - -random_device rd; -default_random_engine eng(rd()); -uniform_real_distribution rand_pos_x; -uniform_real_distribution rand_pos_y; -uniform_real_distribution rand_h; -uniform_real_distribution rand_vel; -uniform_real_distribution rand_acc_r; -uniform_real_distribution rand_acc_t; -uniform_real_distribution rand_acc_z; -uniform_real_distribution rand_color; -uniform_real_distribution rand_scale; -uniform_real_distribution rand_yaw_dot; -uniform_real_distribution rand_yaw; - -ros::Time time_update, time_change; - -void updateCallback(const ros::TimerEvent& e); -void visualizeObj(int id); - -int main(int argc, char** argv) { - ros::init(argc, argv, "dynamic_obj"); - ros::NodeHandle node("~"); - - /* ---------- initialize ---------- */ - node.param("obj_generator/obj_num", obj_num, 20); - node.param("obj_generator/x_size", _x_size, 10.0); - node.param("obj_generator/y_size", _y_size, 10.0); - node.param("obj_generator/h_size", _h_size, 2.0); - node.param("obj_generator/vel", _vel, 2.0); - node.param("obj_generator/yaw_dot", _yaw_dot, 2.0); - node.param("obj_generator/acc_r1", _acc_r1, 2.0); - node.param("obj_generator/acc_r2", _acc_r2, 2.0); - node.param("obj_generator/acc_z", _acc_z, 0.0); - node.param("obj_generator/scale1", _scale1, 0.5); - node.param("obj_generator/scale2", _scale2, 1.0); - node.param("obj_generator/interval", _interval, 100.0); - node.param("obj_generator/input_type", _input_type, 1); - - obj_pub = node.advertise("/dynamic/obj", 10); - for (int i = 0; i < obj_num; ++i) { - ros::Publisher pose_pub = - node.advertise("/dynamic/pose_" + to_string(i), 10); - pose_pubs.push_back(pose_pub); - } - - ros::Timer update_timer = node.createTimer(ros::Duration(1 / 30.0), updateCallback); - cout << "[dynamic]: initialize with " + to_string(obj_num) << " moving obj." << endl; - ros::Duration(1.0).sleep(); - - rand_color = uniform_real_distribution(0.0, 1.0); - rand_pos_x = uniform_real_distribution(-_x_size/2, _x_size/2); - rand_pos_y = uniform_real_distribution(-_y_size/2, _y_size/2); - rand_h = uniform_real_distribution(0.0, _h_size); - rand_vel = uniform_real_distribution(-_vel, _vel); - rand_acc_t = uniform_real_distribution(0.0, 6.28); - rand_acc_r = uniform_real_distribution(_acc_r1, _acc_r2); - rand_acc_z = uniform_real_distribution(-_acc_z, _acc_z); - rand_scale = uniform_real_distribution(_scale1, _scale2); - rand_yaw = uniform_real_distribution(0.0, 2 * 3.141592); - rand_yaw_dot = uniform_real_distribution(-_yaw_dot, _yaw_dot); - - /* ---------- give initial value of each obj ---------- */ - for (int i = 0; i < obj_num; ++i) { - LinearObjModel model; - Eigen::Vector3d pos(rand_pos_x(eng), rand_pos_y(eng), rand_h(eng)); - Eigen::Vector3d vel(rand_vel(eng), rand_vel(eng), 0.0); - Eigen::Vector3d color(rand_color(eng), rand_color(eng), rand_color(eng)); - Eigen::Vector3d scale(rand_scale(eng), 1.5 * rand_scale(eng), 5.0*rand_scale(eng)); - double yaw = rand_yaw(eng); - double yaw_dot = rand_yaw_dot(eng); - - double r, t, z; - r = rand_acc_r(eng); - t = rand_acc_t(eng); - z = rand_acc_z(eng); - Eigen::Vector3d acc(r * cos(t), r * sin(t), z); - - if ( _input_type == 1 ) - { - model.initialize(pos, vel, acc, yaw, yaw_dot, color, scale, _input_type); // Vel input - } - else - { - model.initialize(pos, Eigen::Vector3d(0,0,0), acc, yaw, yaw_dot, color, scale, _input_type); // Acc input - } - model.setLimits(Eigen::Vector3d(_x_size/2, _y_size/2, _h_size), Eigen::Vector2d(0.0, _vel), - Eigen::Vector2d(0, 0)); - obj_models.push_back(model); - } - - time_update = ros::Time::now(); - time_change = ros::Time::now(); - - /* ---------- start loop ---------- */ - ros::spin(); - - return 0; -} - -void updateCallback(const ros::TimerEvent& e) { - ros::Time time_now = ros::Time::now(); - - /* ---------- change input ---------- */ - // double dtc = (time_now - time_change).toSec(); - // if (dtc > _interval) { - // for (int i = 0; i < obj_num; ++i) { - // /* ---------- use acc input ---------- */ - // // double r, t, z; - // // r = rand_acc_r(eng); - // // t = rand_acc_t(eng); - // // z = rand_acc_z(eng); - // // Eigen::Vector3d acc(r * cos(t), r * sin(t), z); - // // obj_models[i].setInput(acc); - - // /* ---------- use vel input ---------- */ - // double vx, vy, vz, yd; - // vx = rand_vel(eng); - // vy = rand_vel(eng); - // vz = 0.0; - // yd = rand_yaw_dot(eng); - - // obj_models[i].setInput(Eigen::Vector3d(vx, vy, vz)); - // obj_models[i].setYawDot(yd); - // } - // time_change = time_now; - // } - - /* ---------- update obj state ---------- */ - double dt = (time_now - time_update).toSec(); - time_update = time_now; - for (int i = 0; i < obj_num; ++i) { - obj_models[i].update(dt); - visualizeObj(i); - ros::Duration(0.000001).sleep(); - } - - /* ---------- collision ---------- */ - // for (int i = 0; i < obj_num; ++i) - // for (int j = i + 1; j < obj_num; ++j) { - // bool collision = LinearObjModel::collide(obj_models[i], obj_models[j]); - // if (collision) { - // double yd1 = rand_yaw_dot(eng); - // double yd2 = rand_yaw_dot(eng); - // obj_models[i].setYawDot(yd1); - // obj_models[j].setYawDot(yd2); - // } - // } -} - -void visualizeObj(int id) { - Eigen::Vector3d pos, color, scale; - pos = obj_models[id].getPosition(); - color = obj_models[id].getColor(); - scale = obj_models[id].getScale(); - double yaw = obj_models[id].getYaw(); - - Eigen::Matrix3d rot; - rot << cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0; - - Eigen::Quaterniond qua; - qua = rot; - - /* ---------- rviz ---------- */ - visualization_msgs::Marker mk; - mk.header.frame_id = "world"; - mk.header.stamp = ros::Time::now(); - mk.type = visualization_msgs::Marker::CUBE; - mk.action = visualization_msgs::Marker::ADD; - mk.id = id; - - mk.scale.x = scale(0), mk.scale.y = scale(1), mk.scale.z = scale(2); - mk.color.a = 1.0, mk.color.r = color(0), mk.color.g = color(1), mk.color.b = color(2); - - mk.pose.orientation.w = qua.w(); - mk.pose.orientation.x = qua.x(); - mk.pose.orientation.y = qua.y(); - mk.pose.orientation.z = qua.z(); - - mk.pose.position.x = pos(0), mk.pose.position.y = pos(1), mk.pose.position.z = pos(2); - - obj_pub.publish(mk); - - /* ---------- pose ---------- */ - geometry_msgs::PoseStamped pose; - pose.header.frame_id = "world"; - pose.header.seq = id; - pose.pose.position.x = pos(0), pose.pose.position.y = pos(1), pose.pose.position.z = pos(2); - pose.pose.orientation.w = 1.0; - pose_pubs[id].publish(pose); -} diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/obj_predictor.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/obj_predictor.cpp deleted file mode 100644 index d960cda4..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/obj_predictor.cpp +++ /dev/null @@ -1,289 +0,0 @@ -/** -* This file is part of Fast-Planner. -* -* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, -* Developed by Boyu Zhou , -* for more information see . -* If you use this code, please cite the respective publications as -* listed on the above website. -* -* Fast-Planner is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* Fast-Planner is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with Fast-Planner. If not, see . -*/ - - - -#include -#include - -namespace fast_planner { -/* ============================== obj history_ ============================== */ - -// int ObjHistory::queue_size_; -// int ObjHistory::skip_num_; -// ros::Time ObjHistory::global_start_time_; - -void ObjHistory::init(int id, int skip_num, int queue_size, ros::Time global_start_time) { - clear(); - skip_ = 0; - obj_idx_ = id; - skip_num_ = skip_num; - queue_size_ = queue_size; - global_start_time_ = global_start_time; -} - -void ObjHistory::poseCallback(const geometry_msgs::PoseStampedConstPtr& msg) { - ++skip_; - if (skip_ < skip_num_) return; - - Eigen::Vector4d pos_t; - pos_t(0) = msg->pose.position.x, pos_t(1) = msg->pose.position.y, pos_t(2) = msg->pose.position.z; - pos_t(3) = (ros::Time::now() - global_start_time_).toSec(); - - history_.push_back(pos_t); - // cout << "idx: " << obj_idx_ << "pos_t: " << pos_t.transpose() << endl; - - if (history_.size() > queue_size_) history_.pop_front(); - - skip_ = 0; -} - -// ObjHistory:: -/* ============================== obj predictor ============================== - */ -ObjPredictor::ObjPredictor(/* args */) { -} - -ObjPredictor::ObjPredictor(ros::NodeHandle& node) { - this->node_handle_ = node; -} - -ObjPredictor::~ObjPredictor() { -} - -void ObjPredictor::init() { - /* get param */ - int queue_size, skip_nums; - - node_handle_.param("prediction/obj_num", obj_num_, 0); - node_handle_.param("prediction/lambda", lambda_, 1.0); - node_handle_.param("prediction/predict_rate", predict_rate_, 1.0); - node_handle_.param("prediction/queue_size", queue_size, 10); - node_handle_.param("prediction/skip_nums", skip_nums, 1); - - predict_trajs_.reset(new vector); - predict_trajs_->resize(obj_num_); - - obj_scale_.reset(new vector); - obj_scale_->resize(obj_num_); - scale_init_.resize(obj_num_); - for (int i = 0; i < obj_num_; i++) - scale_init_[i] = false; - - /* subscribe to pose */ - ros::Time t_now = ros::Time::now(); - for (int i = 0; i < obj_num_; i++) { - shared_ptr obj_his(new ObjHistory); - - obj_his->init(i, skip_nums, queue_size, t_now); - obj_histories_.push_back(obj_his); - - ros::Subscriber pose_sub = node_handle_.subscribe( - "/dynamic/pose_" + std::to_string(i), 10, &ObjHistory::poseCallback, obj_his.get()); - - pose_subs_.push_back(pose_sub); - - predict_trajs_->at(i).setGlobalStartTime(t_now); - } - - marker_sub_ = node_handle_.subscribe("/dynamic/obj", 10, - &ObjPredictor::markerCallback, this); - - /* update prediction */ - predict_timer_ = - node_handle_.createTimer(ros::Duration(1 / predict_rate_), &ObjPredictor::predictCallback, this); -} - -ObjPrediction ObjPredictor::getPredictionTraj() { - return this->predict_trajs_; -} - -ObjScale ObjPredictor::getObjScale() { - return this->obj_scale_; -} - -void ObjPredictor::predictPolyFit() { - /* iterate all obj */ - for (int i = 0; i < obj_num_; i++) { - /* ---------- write A and b ---------- */ - Eigen::Matrix A; - Eigen::Matrix temp; - Eigen::Matrix bm[3]; // poly coefficent - vector> pm(3); - - A.setZero(); - for (int i = 0; i < 3; ++i) - bm[i].setZero(); - - /* ---------- estimation error ---------- */ - list his; - obj_histories_[i]->getHistory(his); - for (list::iterator it = his.begin(); it != his.end(); ++it) { - Eigen::Vector3d qi = (*it).head(3); - double ti = (*it)(3); - - /* A */ - temp << 1.0, ti, pow(ti, 2), pow(ti, 3), pow(ti, 4), pow(ti, 5); - for (int j = 0; j < 6; ++j) - A.row(j) += 2.0 * pow(ti, j) * temp.transpose(); - - /* b */ - for (int dim = 0; dim < 3; ++dim) - bm[dim] += 2.0 * qi(dim) * temp; - } - - /* ---------- acceleration regulator ---------- */ - double t1 = his.front()(3); - double t2 = his.back()(3); - - temp << 0.0, 0.0, 2 * t1 - 2 * t2, 3 * pow(t1, 2) - 3 * pow(t2, 2), 4 * pow(t1, 3) - 4 * pow(t2, 3), - 5 * pow(t1, 4) - 5 * pow(t2, 4); - A.row(2) += -4 * lambda_ * temp.transpose(); - - temp << 0.0, 0.0, pow(t1, 2) - pow(t2, 2), 2 * pow(t1, 3) - 2 * pow(t2, 3), - 3 * pow(t1, 4) - 3 * pow(t2, 4), 4 * pow(t1, 5) - 4 * pow(t2, 5); - A.row(3) += -12 * lambda_ * temp.transpose(); - - temp << 0.0, 0.0, 20 * pow(t1, 3) - 20 * pow(t2, 3), 45 * pow(t1, 4) - 45 * pow(t2, 4), - 72 * pow(t1, 5) - 72 * pow(t2, 5), 100 * pow(t1, 6) - 100 * pow(t2, 6); - A.row(4) += -4.0 / 5.0 * lambda_ * temp.transpose(); - - temp << 0.0, 0.0, 35 * pow(t1, 4) - 35 * pow(t2, 4), 84 * pow(t1, 5) - 84 * pow(t2, 5), - 140 * pow(t1, 6) - 140 * pow(t2, 6), 200 * pow(t1, 7) - 200 * pow(t2, 7); - A.row(5) += -4.0 / 7.0 * lambda_ * temp.transpose(); - - /* ---------- solve ---------- */ - for (int j = 0; j < 3; j++) { - pm[j] = A.colPivHouseholderQr().solve(bm[j]); - } - - /* ---------- update prediction container ---------- */ - predict_trajs_->at(i).setPolynomial(pm); - predict_trajs_->at(i).setTime(t1, t2); - } -} - -void ObjPredictor::predictCallback(const ros::TimerEvent& e) { - // predictPolyFit(); - predictConstVel(); -} - -void ObjPredictor::markerCallback(const visualization_msgs::MarkerConstPtr& msg) { - int idx = msg->id; - (*obj_scale_)[idx](0) = msg->scale.x; - (*obj_scale_)[idx](1) = msg->scale.y; - (*obj_scale_)[idx](2) = msg->scale.z; - - scale_init_[idx] = true; - - int finish_num = 0; - for (int i = 0; i < obj_num_; i++) { - if (scale_init_[i]) finish_num++; - } - - if (finish_num == obj_num_) { - marker_sub_.shutdown(); - } -} - -void ObjPredictor::predictConstVel() { - for (int i = 0; i < obj_num_; i++) { - /* ---------- get the last two point ---------- */ - list his; - obj_histories_[i]->getHistory(his); - // if ( i==0 ) - // { - // cout << "his.size()=" << his.size() << endl; - // for ( auto hi:his ) - // { - // cout << hi.transpose() << endl; - // } - // } - list::iterator list_it = his.end(); - - /* ---------- test iteration ---------- */ - // cout << "----------------------------" << endl; - // for (auto v4d : his) - // cout << "v4d: " << v4d.transpose() << endl; - - Eigen::Vector3d q1, q2; - double t1, t2; - - --list_it; - q2 = (*list_it).head(3); - t2 = (*list_it)(3); - - --list_it; - q1 = (*list_it).head(3); - t1 = (*list_it)(3); - - Eigen::Matrix p01, q12; - q12.row(0) = q1.transpose(); - q12.row(1) = q2.transpose(); - - Eigen::Matrix At12; - At12 << 1, t1, 1, t2; - - p01 = At12.inverse() * q12; - - vector> polys(3); - for (int j = 0; j < 3; ++j) { - polys[j].setZero(); - polys[j].head(2) = p01.col(j); - } - - // if ( i==0 ) - // { - // cout << "q1=" << q1.transpose() << " t1=" << t1 << " q2=" << q2.transpose() << " t2=" << t2 << endl; - // cout << "polys=" << polys[0].transpose() << endl; - // } - - predict_trajs_->at(i).setPolynomial(polys); - predict_trajs_->at(i).setTime(t1, t2); - } -} - -Eigen::Vector3d ObjPredictor::evaluatePoly(int obj_id, double time) -{ - if ( obj_id < obj_num_ ) - { - return predict_trajs_->at(obj_id).evaluate(time); - } - - double MAX = std::numeric_limits::max(); - return Eigen::Vector3d(MAX, MAX, MAX); -} - -Eigen::Vector3d ObjPredictor::evaluateConstVel(int obj_id, double time) -{ - if ( obj_id < obj_num_ ) - { - return predict_trajs_->at(obj_id).evaluateConstVel(time); - } - - double MAX = std::numeric_limits::max(); - return Eigen::Vector3d(MAX, MAX, MAX); -} - -// ObjPredictor:: -} // namespace fast_planner \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/raycast.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/raycast.cpp deleted file mode 100644 index 56ffdbf1..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_env/src/raycast.cpp +++ /dev/null @@ -1,321 +0,0 @@ -#include -#include -#include -#include - -int signum(int x) { - return x == 0 ? 0 : x < 0 ? -1 : 1; -} - -double mod(double value, double modulus) { - return fmod(fmod(value, modulus) + modulus, modulus); -} - -double intbound(double s, double ds) { - // Find the smallest positive t such that s+t*ds is an integer. - if (ds < 0) { - return intbound(-s, -ds); - } else { - s = mod(s, 1); - // problem is now s+t*ds = 1 - return (1 - s) / ds; - } -} - -void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, - const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output) { - // std::cout << start << ' ' << end << std::endl; - // From "A Fast Voxel Traversal Algorithm for Ray Tracing" - // by John Amanatides and Andrew Woo, 1987 - // - // - // Extensions to the described algorithm: - // • Imposed a distance limit. - // • The face passed through to reach the current cube is provided to - // the callback. - - // The foundation of this algorithm is a parameterized representation of - // the provided ray, - // origin + t * direction, - // except that t is not actually stored; rather, at any given point in the - // traversal, we keep track of the *greater* t values which we would have - // if we took a step sufficient to cross a cube boundary along that axis - // (i.e. change the integer part of the coordinate) in the variables - // tMaxX, tMaxY, and tMaxZ. - - // Cube containing origin point. - int x = (int)std::floor(start.x()); - int y = (int)std::floor(start.y()); - int z = (int)std::floor(start.z()); - int endX = (int)std::floor(end.x()); - int endY = (int)std::floor(end.y()); - int endZ = (int)std::floor(end.z()); - Eigen::Vector3d direction = (end - start); - double maxDist = direction.squaredNorm(); - - // Break out direction vector. - double dx = endX - x; - double dy = endY - y; - double dz = endZ - z; - - // Direction to increment x,y,z when stepping. - int stepX = (int)signum((int)dx); - int stepY = (int)signum((int)dy); - int stepZ = (int)signum((int)dz); - - // See description above. The initial values depend on the fractional - // part of the origin. - double tMaxX = intbound(start.x(), dx); - double tMaxY = intbound(start.y(), dy); - double tMaxZ = intbound(start.z(), dz); - - // The change in t when taking a step (always positive). - double tDeltaX = ((double)stepX) / dx; - double tDeltaY = ((double)stepY) / dy; - double tDeltaZ = ((double)stepZ) / dz; - - // Avoids an infinite loop. - if (stepX == 0 && stepY == 0 && stepZ == 0) return; - - double dist = 0; - while (true) { - if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) { - output[output_points_cnt](0) = x; - output[output_points_cnt](1) = y; - output[output_points_cnt](2) = z; - - output_points_cnt++; - dist = sqrt((x - start(0)) * (x - start(0)) + (y - start(1)) * (y - start(1)) + - (z - start(2)) * (z - start(2))); - - if (dist > maxDist) return; - - /* if (output_points_cnt > 1500) { - std::cerr << "Error, too many racyast voxels." << - std::endl; - throw std::out_of_range("Too many raycast voxels"); - }*/ - } - - if (x == endX && y == endY && z == endZ) break; - - // tMaxX stores the t-value at which we cross a cube boundary along the - // X axis, and similarly for Y and Z. Therefore, choosing the least tMax - // chooses the closest cube boundary. Only the first case of the four - // has been commented in detail. - if (tMaxX < tMaxY) { - if (tMaxX < tMaxZ) { - // Update which cube we are now in. - x += stepX; - // Adjust tMaxX to the next X-oriented boundary crossing. - tMaxX += tDeltaX; - } else { - z += stepZ; - tMaxZ += tDeltaZ; - } - } else { - if (tMaxY < tMaxZ) { - y += stepY; - tMaxY += tDeltaY; - } else { - z += stepZ; - tMaxZ += tDeltaZ; - } - } - } -} - -void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, - const Eigen::Vector3d& max, std::vector* output) { - // std::cout << start << ' ' << end << std::endl; - // From "A Fast Voxel Traversal Algorithm for Ray Tracing" - // by John Amanatides and Andrew Woo, 1987 - // - // - // Extensions to the described algorithm: - // • Imposed a distance limit. - // • The face passed through to reach the current cube is provided to - // the callback. - - // The foundation of this algorithm is a parameterized representation of - // the provided ray, - // origin + t * direction, - // except that t is not actually stored; rather, at any given point in the - // traversal, we keep track of the *greater* t values which we would have - // if we took a step sufficient to cross a cube boundary along that axis - // (i.e. change the integer part of the coordinate) in the variables - // tMaxX, tMaxY, and tMaxZ. - - // Cube containing origin point. - int x = (int)std::floor(start.x()); - int y = (int)std::floor(start.y()); - int z = (int)std::floor(start.z()); - int endX = (int)std::floor(end.x()); - int endY = (int)std::floor(end.y()); - int endZ = (int)std::floor(end.z()); - Eigen::Vector3d direction = (end - start); - double maxDist = direction.squaredNorm(); - - // Break out direction vector. - double dx = endX - x; - double dy = endY - y; - double dz = endZ - z; - - // Direction to increment x,y,z when stepping. - int stepX = (int)signum((int)dx); - int stepY = (int)signum((int)dy); - int stepZ = (int)signum((int)dz); - - // See description above. The initial values depend on the fractional - // part of the origin. - double tMaxX = intbound(start.x(), dx); - double tMaxY = intbound(start.y(), dy); - double tMaxZ = intbound(start.z(), dz); - - // The change in t when taking a step (always positive). - double tDeltaX = ((double)stepX) / dx; - double tDeltaY = ((double)stepY) / dy; - double tDeltaZ = ((double)stepZ) / dz; - - output->clear(); - - // Avoids an infinite loop. - if (stepX == 0 && stepY == 0 && stepZ == 0) return; - - double dist = 0; - while (true) { - if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) { - output->push_back(Eigen::Vector3d(x, y, z)); - - dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm(); - - if (dist > maxDist) return; - - if (output->size() > 1500) { - std::cerr << "Error, too many racyast voxels." << std::endl; - throw std::out_of_range("Too many raycast voxels"); - } - } - - if (x == endX && y == endY && z == endZ) break; - - // tMaxX stores the t-value at which we cross a cube boundary along the - // X axis, and similarly for Y and Z. Therefore, choosing the least tMax - // chooses the closest cube boundary. Only the first case of the four - // has been commented in detail. - if (tMaxX < tMaxY) { - if (tMaxX < tMaxZ) { - // Update which cube we are now in. - x += stepX; - // Adjust tMaxX to the next X-oriented boundary crossing. - tMaxX += tDeltaX; - } else { - z += stepZ; - tMaxZ += tDeltaZ; - } - } else { - if (tMaxY < tMaxZ) { - y += stepY; - tMaxY += tDeltaY; - } else { - z += stepZ; - tMaxZ += tDeltaZ; - } - } - } -} - -bool RayCaster::setInput(const Eigen::Vector3d& start, - const Eigen::Vector3d& end /* , const Eigen::Vector3d& min, - const Eigen::Vector3d& max */) { - start_ = start; - end_ = end; - // max_ = max; - // min_ = min; - - x_ = (int)std::floor(start_.x()); - y_ = (int)std::floor(start_.y()); - z_ = (int)std::floor(start_.z()); - endX_ = (int)std::floor(end_.x()); - endY_ = (int)std::floor(end_.y()); - endZ_ = (int)std::floor(end_.z()); - direction_ = (end_ - start_); - maxDist_ = direction_.squaredNorm(); - - // Break out direction vector. - dx_ = endX_ - x_; - dy_ = endY_ - y_; - dz_ = endZ_ - z_; - - // Direction to increment x,y,z when stepping. - stepX_ = (int)signum((int)dx_); - stepY_ = (int)signum((int)dy_); - stepZ_ = (int)signum((int)dz_); - - // See description above. The initial values depend on the fractional - // part of the origin. - tMaxX_ = intbound(start_.x(), dx_); - tMaxY_ = intbound(start_.y(), dy_); - tMaxZ_ = intbound(start_.z(), dz_); - - // The change in t when taking a step (always positive). - tDeltaX_ = ((double)stepX_) / dx_; - tDeltaY_ = ((double)stepY_) / dy_; - tDeltaZ_ = ((double)stepZ_) / dz_; - - dist_ = 0; - - step_num_ = 0; - - // Avoids an infinite loop. - if (stepX_ == 0 && stepY_ == 0 && stepZ_ == 0) - return false; - else - return true; -} - -bool RayCaster::step(Eigen::Vector3d& ray_pt) { - // if (x_ >= min_.x() && x_ < max_.x() && y_ >= min_.y() && y_ < max_.y() && - // z_ >= min_.z() && z_ < - // max_.z()) - ray_pt = Eigen::Vector3d(x_, y_, z_); - - // step_num_++; - - // dist_ = (Eigen::Vector3d(x_, y_, z_) - start_).squaredNorm(); - - if (x_ == endX_ && y_ == endY_ && z_ == endZ_) { - return false; - } - - // if (dist_ > maxDist_) - // { - // return false; - // } - - // tMaxX stores the t-value at which we cross a cube boundary along the - // X axis, and similarly for Y and Z. Therefore, choosing the least tMax - // chooses the closest cube boundary. Only the first case of the four - // has been commented in detail. - if (tMaxX_ < tMaxY_) { - if (tMaxX_ < tMaxZ_) { - // Update which cube we are now in. - x_ += stepX_; - // Adjust tMaxX to the next X-oriented boundary crossing. - tMaxX_ += tDeltaX_; - } else { - z_ += stepZ_; - tMaxZ_ += tDeltaZ_; - } - } else { - if (tMaxY_ < tMaxZ_) { - y_ += stepY_; - tMaxY_ += tDeltaY_; - } else { - z_ += stepZ_; - tMaxZ_ += tDeltaZ_; - } - } - - return true; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/CMakeLists.txt b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/CMakeLists.txt deleted file mode 100644 index 0d182e6a..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(ego_planner) - -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 - std_msgs - geometry_msgs - quadrotor_msgs - plan_env - path_searching - bspline_opt - traj_utils - message_generation - cv_bridge - prometheus_msgs -) - -# catkin_package(CATKIN_DEPENDS message_runtime) -catkin_package( - INCLUDE_DIRS include - LIBRARIES ego_planner - CATKIN_DEPENDS plan_env path_searching bspline_opt traj_utils -# DEPENDS system_lib -) - -include_directories( - include - SYSTEM - ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include - ${EIGEN3_INCLUDE_DIR} - ${PCL_INCLUDE_DIRS} - ${PROJECT_SOURCE_DIR}/../../uav_control/include - ${PROJECT_SOURCE_DIR}/../../common/include -) - - -add_executable(ego_planner_node - src/ego_planner_node.cpp - src/ego_replan_fsm.cpp - src/planner_manager.cpp - ) -target_link_libraries(ego_planner_node - ${catkin_LIBRARIES} - ) -#add_dependencies(ego_planner_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -add_executable(traj_server src/traj_server.cpp) -target_link_libraries(traj_server ${catkin_LIBRARIES}) - - -add_executable(traj_server_for_prometheus src_for_prometheus/traj_server_for_prometheus.cpp) -target_link_libraries(traj_server_for_prometheus ${catkin_LIBRARIES}) - -add_executable(ego_goal_pub src_for_prometheus/ego_goal_pub.cpp) -target_link_libraries(ego_goal_pub ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/include/plan_manage/ego_replan_fsm.h b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/include/plan_manage/ego_replan_fsm.h deleted file mode 100644 index 35c17217..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/include/plan_manage/ego_replan_fsm.h +++ /dev/null @@ -1,133 +0,0 @@ -#ifndef _REBO_REPLAN_FSM_H_ -#define _REBO_REPLAN_FSM_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -using std::vector; - -namespace ego_planner -{ - - class EGOReplanFSM - { - - private: - /* ---------- flag ---------- */ - //规划器状态 - enum FSM_EXEC_STATE - { - INIT, - WAIT_TARGET, - GEN_NEW_TRAJ, - REPLAN_TRAJ, - EXEC_TRAJ, - EMERGENCY_STOP, - SEQUENTIAL_START - }; - // 目标点类型 - enum TARGET_TYPE - { - MANUAL_TARGET = 1, - PRESET_TARGET = 2, - REFENCE_PATH = 3 - }; - - /* planning utils */ - EGOPlannerManager::Ptr planner_manager_; - PlanningVisualization::Ptr visualization_; - traj_utils::DataDisp data_disp_; - traj_utils::MultiBsplines multi_bspline_msgs_buf_; - - /* parameters */ - int target_type_; // 1 mannual select, 2 hard code - double no_replan_thresh_, replan_thresh_; - double waypoints_[10][3]; - - int waypoint_num_, wp_id_; - double planning_horizen_, planning_horizen_time_; - double emergency_time_; - bool flag_realworld_experiment_; - bool enable_fail_safe_; - - /* planning data */ - bool have_trigger_, have_target_, have_odom_, have_new_target_, have_recv_pre_agent_; - FSM_EXEC_STATE exec_state_; - int continously_called_times_{0}; - - Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state - Eigen::Quaterniond odom_orient_; - - Eigen::Vector3d init_pt_, start_pt_, start_vel_, start_acc_, start_yaw_; // start state - Eigen::Vector3d end_pt_, end_vel_; // goal state - Eigen::Vector3d local_target_pt_, local_target_vel_; // local target state - std::vector wps_; - int current_wp_; - - bool flag_escape_emergency_; - - /* ROS utils */ - ros::NodeHandle node_; - ros::Timer exec_timer_, safety_timer_; - ros::Subscriber waypoint_sub_, odom_sub_, swarm_trajs_sub_, broadcast_bspline_sub_, trigger_sub_; - ros::Publisher replan_pub_, new_pub_, bspline_pub_, data_disp_pub_, swarm_trajs_pub_, broadcast_bspline_pub_; - - /* helper functions */ - bool callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj); // front-end and back-end method - bool callEmergencyStop(Eigen::Vector3d stop_pos); // front-end and back-end method - bool planFromGlobalTraj(const int trial_times = 1); - bool planFromCurrentTraj(const int trial_times = 1); - - /* return value: std::pair< Times of the same state be continuously called, current continuously called state > */ - void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call); - std::pair timesOfConsecutiveStateCalls(); - void printFSMExecState(); - - void readGivenWps(); - void planNextWaypoint(const Eigen::Vector3d next_wp); - void getLocalTarget(); - - /* ROS functions */ - void execFSMCallback(const ros::TimerEvent &e); - void checkCollisionCallback(const ros::TimerEvent &e); - void waypointCallback(const geometry_msgs::PoseStampedPtr &msg); - void triggerCallback(const geometry_msgs::PoseStampedPtr &msg); - void odometryCallback(const nav_msgs::OdometryConstPtr &msg); - void swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg); - void BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg); - - bool checkCollision(); - void publishSwarmTrajs(bool startup_pub); - - public: - EGOReplanFSM(/* args */) - { - } - ~EGOReplanFSM() - { - } - - void init(ros::NodeHandle &nh); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - -} // namespace ego_planner - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/include/plan_manage/planner_manager.h b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/include/plan_manage/planner_manager.h deleted file mode 100644 index ce546421..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/include/plan_manage/planner_manager.h +++ /dev/null @@ -1,85 +0,0 @@ -#ifndef _PLANNER_MANAGER_H_ -#define _PLANNER_MANAGER_H_ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ego_planner -{ - - // Fast Planner Manager - // Key algorithms of mapping and planning are called - - class EGOPlannerManager - { - // SECTION stable - public: - EGOPlannerManager(); - ~EGOPlannerManager(); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /* main planning interface */ - bool reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc, - Eigen::Vector3d end_pt, Eigen::Vector3d end_vel, bool flag_polyInit, bool flag_randomPolyTraj); - bool EmergencyStop(Eigen::Vector3d stop_pos); - bool planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc); - bool planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, - const std::vector &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc); - - void initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis = NULL); - - void deliverTrajToOptimizer(void) { bspline_optimizer_->setSwarmTrajs(&swarm_trajs_buf_); }; - - void setDroneIdtoOpt(void) { bspline_optimizer_->setDroneId(pp_.drone_id); } - - double getSwarmClearance(void) { return bspline_optimizer_->getSwarmClearance(); } - - bool checkCollision(int drone_id); - - - PlanParameters pp_; - LocalTrajData local_data_; - GlobalTrajData global_data_; - GridMap::Ptr grid_map_; - fast_planner::ObjPredictor::Ptr obj_predictor_; - SwarmTrajData swarm_trajs_buf_; - - private: - /* main planning algorithms & modules */ - PlanningVisualization::Ptr visualization_; - - // ros::Publisher obj_pub_; //zx-todo - - BsplineOptimizer::Ptr bspline_optimizer_; - - int continous_failures_count_{0}; - - void updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now); - - void reparamBspline(UniformBspline &bspline, vector &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt, - double &time_inc); - - bool refineTrajAlgo(UniformBspline &traj, vector &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points); - - // !SECTION stable - - // SECTION developing - - public: - typedef unique_ptr Ptr; - - // !SECTION - }; -} // namespace ego_planner - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/advanced_param.xml b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/advanced_param.xml deleted file mode 100644 index 7f7b438a..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/advanced_param.xml +++ /dev/null @@ -1,160 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/default.rviz b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/default.rviz deleted file mode 100644 index 5a1a1969..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/default.rviz +++ /dev/null @@ -1,3055 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /simulation_map1/Autocompute Value Bounds1 - - /drone01/Planning1 - - /drone01/Planning1/drone_path1 - - /drone01/Planning1/drone_path1/Offset1 - - /drone01/Mapping1 - - /drone01/Mapping1/map inflate1 - - /drone11/Planning1 - - /drone11/Planning1/drone_path1 - - /drone11/Mapping1 - - /drone11/Mapping1/map inflate1 - - /drone11/Simulation1 - - /drone21/Planning1 - - /drone21/Planning1/drone_path1 - - /drone21/Mapping1 - - /drone21/Mapping1/map inflate1 - - /drone31/Planning1 - - /drone31/Planning1/drone_path1 - - /drone31/Mapping1 - - /drone31/Mapping1/map inflate1 - - /drone41/Planning1 - - /drone41/Planning1/optimal_traj1 - - /drone41/Planning1/drone_path1 - - /drone41/Mapping1 - - /drone41/Mapping1/map inflate1 - - /drone51/Planning1 - - /drone51/Planning1/drone_path1 - - /drone51/Mapping1 - - /drone51/Mapping1/map inflate1 - - /drone61/Planning1 - - /drone61/Planning1/drone_path1 - - /drone61/Mapping1 - - /drone61/Mapping1/map inflate1 - - /drone71/Planning1 - - /drone71/Planning1/InitTraj1 - - /drone71/Planning1/drone_path1 - - /drone71/Mapping1 - - /drone71/Mapping1/map inflate1 - - /drone71/Simulation1 - - /drone71/Simulation1/robot1 - - /drone81/Planning1 - - /drone81/Planning1/drone_path1 - - /drone81/Mapping1 - - /drone81/Mapping1/map inflate1 - - /drone81/Simulation1/robot1 - - /drone91/Planning1 - - /drone91/Planning1/drone_path1 - - /drone91/Mapping1 - - /drone91/Mapping1/map inflate1 - - /drone91/Simulation1 - - /drone91/Simulation1/robot1 - - /drone101/Planning1 - - /drone101/Planning1/optimal_traj1 - - /drone101/Planning1/drone_path1 - - /drone101/Simulation1 - - /drone101/Simulation1/robot1 - - /drone111/Planning1 - - /drone111/Planning1/optimal_traj1 - - /drone111/Simulation1 - - /drone111/Simulation1/robot1 - - /drone121/Planning1 - - /drone121/Planning1/optimal_traj1 - - /drone121/Planning1/drone_path1 - - /drone121/Simulation1 - - /drone121/Simulation1/robot1 - - /drone131/Planning1 - - /drone131/Planning1/optimal_traj1 - - /drone131/Planning1/drone_path1 - - /drone131/Simulation1/robot1 - - /drone141/Planning1 - - /drone141/Planning1/drone_path1 - - /drone141/Simulation1/robot1 - - /drone151/Planning1 - - /drone151/Planning1/optimal_traj1 - - /drone151/Planning1/drone_path1 - - /drone151/Simulation1 - - /drone151/Simulation1/robot1 - - /drone161/Planning1 - - /drone161/Planning1/optimal_traj1 - - /drone161/Planning1/drone_path1 - - /drone161/Simulation1 - - /drone161/Simulation1/robot1 - - /drone171/Planning1 - - /drone171/Planning1/optimal_traj1 - - /drone171/Planning1/drone_path1 - - /drone171/Simulation1 - - /drone171/Simulation1/robot1 - - /drone181/Planning1 - - /drone181/Planning1/optimal_traj1 - - /drone181/Planning1/drone_path1 - - /drone181/Simulation1 - - /drone181/Simulation1/robot1 - - /drone191/Planning1 - - /drone191/Planning1/goal_point1 - - /drone191/Planning1/optimal_traj1 - - /drone191/Planning1/drone_path1 - - /drone191/Simulation1 - - /drone191/Simulation1/robot1 - - /drone201/Planning1 - - /drone201/Planning1/drone_path1 - - /drone201/Simulation1 - - /drone201/Simulation1/robot1 - Splitter Ratio: 0.43611112236976624 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: simulation_map -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 1 - Class: rviz/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 0.15000000596046448 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.9000000953674316 - Min Value: 0 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 85; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: simulation_map - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Boxes - Topic: /map_generator/global_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_0_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_0_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_0_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 29; 108; 212 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_0_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.8399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 29; 108; 212 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_0_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_0_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_0_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone0 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_1_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_1_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_1_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_1_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_1_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_1_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_1_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone1 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_2_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_2_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_2_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_2_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_2_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_2_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_2_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone2 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_3_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_3_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_3_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 0; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_3_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_3_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_3_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_3_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone3 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_4_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_4_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_4_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_4_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_4_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_4_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_4_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone4 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_5_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_5_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_5_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_5_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_5_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_5_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone5 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_6_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_6_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_6_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 190; 120; 121 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_6_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 190; 120; 121 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_6_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_6_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_6_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone6 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_7_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_7_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_7_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_7_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_5_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_7_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone7 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_8_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_8_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_8_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 226; 0; 158 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_8_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 226; 0; 158 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_8_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_8_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone8 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_9_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 85; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_9_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_9_odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone9 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_10_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 29; 108; 212 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_10_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_10_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone10 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_11_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_11_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_11_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone11 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_12_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_12_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_12_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone12 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_13_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 0; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_13_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_13_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone13 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_14_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_14_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_14_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone14 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_15_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_15_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_15_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone15 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_16_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 190; 120; 121 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_16_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_16_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone16 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_17_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_17_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_17_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone17 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_18_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 226; 0; 158 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_18_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_18_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone18 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_19_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_19_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 85; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_19_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_19_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone19 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_20_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /drone_9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 20; 106; 172 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /drone_20_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone_20_odom_visualization/robot - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /drone_5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone20 - - Class: rviz/Image - Enabled: false - Image Topic: /drone_0_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 52.77322769165039 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -10.308119773864746 - Y: -0.9097803831100464 - Z: -9.5367431640625e-07 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 - Target Frame: - Yaw: 3.1415886878967285 - Saved: - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4000000059604645 - Position: - X: -11 - Y: 0 - Z: 8 - Roll: 0 - Target Frame: my_view - Yaw: 0 - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5 - Position: - X: -10 - Y: 0 - Z: 10 - Roll: 0 - Target Frame: my_view - Yaw: 0 - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Position: - X: -10 - Y: 0 - Z: 10 - Roll: 0 - Target Frame: my_view - Yaw: 0 -Window Geometry: - Displays: - collapsed: true - Height: 1241 - Hide Left Dock: true - Hide Right Dock: false - Image: - collapsed: true - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1102 - X: 3339 - Y: 75 - depth: - collapsed: false diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/run_in_sim.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/run_in_sim.launch deleted file mode 100644 index 0eb4ad79..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/run_in_sim.launch +++ /dev/null @@ -1,135 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - > - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/rviz.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/rviz.launch deleted file mode 100644 index 667873df..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/rviz.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/simple_run.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/simple_run.launch deleted file mode 100644 index 44ab7106..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/simple_run.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/simulator.xml b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/simulator.xml deleted file mode 100644 index 9950ea0d..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/simulator.xml +++ /dev/null @@ -1,141 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/single_run_in_sim.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/single_run_in_sim.launch deleted file mode 100644 index 14639cc9..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/single_run_in_sim.launch +++ /dev/null @@ -1,117 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/swarm.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/swarm.launch deleted file mode 100644 index 1cc2db55..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/swarm.launch +++ /dev/null @@ -1,205 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/swarm_large.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/swarm_large.launch deleted file mode 100644 index d3794d1f..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch/swarm_large.launch +++ /dev/null @@ -1,412 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/advanced_param.xml b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/advanced_param.xml deleted file mode 100644 index d0b67654..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/advanced_param.xml +++ /dev/null @@ -1,156 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego.rviz b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego.rviz deleted file mode 100644 index e06b824b..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego.rviz +++ /dev/null @@ -1,608 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /simulation_map1/Autocompute Value Bounds1 - - /drone11 - - /drone11/Planning1 - - /drone11/Mapping1 - - /drone11/Simulation1 - - /drone11/Simulation1/Odometry1/Shape1 - - /drone21 - - /drone21/Planning1/drone_path1 - - /drone21/Mapping1 - - /drone21/Mapping1/map_generator_local1/Status1 - - /drone21/Simulation1/Odometry1/Shape1 - - /drone21/Simulation1/Odometry1/Covariance1 - - /drone21/Simulation1/robot1 - Splitter Ratio: 0.43611112236976624 - Tree Height: 922 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: map_generator_local -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Class: rviz/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 0.15000000596046448 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 3 - Min Value: 0.009999999776482582 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 85; 170; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: simulation_map - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Boxes - Topic: /map_generator/global_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav1_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav1_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav1_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav1_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav1_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 204; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav1/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 1 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.20000000298023224 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /uav1/prometheus/drone_odom - Unreliable: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav1/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav1_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone1 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav2_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav2_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav2_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav2_odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 115; 210; 22 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: /uav2_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 92; 53; 102 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav2/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 1 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.20000000298023224 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /uav2/prometheus/drone_odom - Unreliable: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav2/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav2_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone2 - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 19.938554763793945 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.7258191108703613 - Y: -0.35111796855926514 - Z: -3.442609158810228e-5 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.9197959899902344 - Target Frame: - Value: ThirdPersonFollower (rviz) - Yaw: 3.2434911727905273 - Saved: - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4000000059604645 - Position: - X: -11 - Y: 0 - Z: 8 - Target Frame: my_view - Value: FPS (rviz) - Yaw: 0 - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5 - Position: - X: -10 - Y: 0 - Z: 10 - Target Frame: my_view - Value: FPS (rviz) - Yaw: 0 - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Position: - X: -10 - Y: 0 - Z: 10 - Target Frame: my_view - Value: FPS (rviz) - Yaw: 0 -Window Geometry: - Displays: - collapsed: false - Height: 1476 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 2223 - X: 2724 - Y: 74 - depth: - collapsed: false diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego_control_config.yaml b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego_control_config.yaml deleted file mode 100644 index df314e3a..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego_control_config.yaml +++ /dev/null @@ -1,40 +0,0 @@ - -# 质量，单位：kg -quad_mass : 1.5 -# hov_percent会影响最大推力 -hov_percent : 0.47 - -Limit: - # 位置比例控制 - pxy_int_max: 5.0 - pz_int_max: 10.0 - -hover_gain: - # 位置比例控制 - Kp_xy: 2.0 - Kp_z: 2.0 - # 速度比例控制 - Kv_xy: 2.0 - Kv_z: 2.0 - # 速度积分控制? - Kvi_xy: 0.3 - Kvi_z: 0.3 - # 加速度前馈系数 - Ka_xy: 1.0 - Ka_z: 1.0 - tilt_angle_max : 10.0 - -track_gain: - # 位置比例控制 - Kp_xy: 2.0 - Kp_z: 2.0 - # 速度比例控制 - Kv_xy: 2.0 - Kv_z: 2.0 - # 速度积分控制? - Kvi_xy: 0.1 - Kvi_z: 0.1 - # 加速度前馈系数 - Ka_xy: 1.0 - Ka_z: 1.0 - tilt_angle_max : 10.0 \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/real_ego_planner_basic.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/real_ego_planner_basic.launch deleted file mode 100644 index b262ba66..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/real_ego_planner_basic.launch +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/rviz_cxy_case2.rviz b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/rviz_cxy_case2.rviz deleted file mode 100644 index a8de0048..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/rviz_cxy_case2.rviz +++ /dev/null @@ -1,7575 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /simulation_map1/Autocompute Value Bounds1 - - /drone21/Planning1 - - /drone21/Planning1/drone_path1 - - /drone21/Mapping1 - - /drone31/Planning1 - - /drone41/Planning1/optimal_traj1 - - /drone41/Planning1/drone_path1 - - /drone41/Mapping1 - - /drone41/Mapping1/map inflate1 - - /drone41/Simulation1 - - /drone51/Planning1 - - /drone51/Planning1/drone_path1 - - /drone51/Mapping1 - - /drone51/Mapping1/map inflate1 - - /drone51/Simulation1 - - /drone51/Simulation1/robot1 - - /drone61/Planning1 - - /drone61/Planning1/drone_path1 - - /drone61/Mapping1 - - /drone61/Mapping1/map inflate1 - - /drone61/Simulation1/robot1 - - /drone71/Planning1 - - /drone71/Mapping1/map inflate1 - - /drone71/Simulation1 - - /drone71/Simulation1/robot1 - - /drone81/Planning1 - - /drone81/Planning1/drone_path1 - - /drone81/Mapping1 - - /drone81/Mapping1/map inflate1 - - /drone81/Simulation1 - - /drone81/Simulation1/robot1 - - /drone91/Planning1 - - /drone91/Planning1/goal_point1/Status1 - - /drone91/Simulation1/robot1 - - /drone101/Planning1 - - /drone101/Planning1/goal_point1 - - /drone101/Planning1/optimal_traj1 - - /drone101/Simulation1 - - /drone101/Simulation1/robot1 - - /drone111/Planning1 - - /drone111/Planning1/goal_point1 - - /drone111/Planning1/optimal_traj1 - - /drone111/Planning1/drone_path1 - - /drone111/Mapping1 - - /drone111/Mapping1/map inflate1 - - /drone111/Simulation1 - - /drone111/Simulation1/robot1 - - /drone121/Planning1 - - /drone121/Planning1/goal_point1 - - /drone121/Planning1/optimal_traj1 - - /drone121/Planning1/drone_path1 - - /drone121/Simulation1 - - /drone121/Simulation1/robot1 - - /drone131/Planning1 - - /drone131/Planning1/goal_point1 - - /drone131/Planning1/optimal_traj1 - - /drone131/Planning1/drone_path1 - - /drone131/Simulation1 - - /drone131/Simulation1/robot1 - - /drone141/Planning1 - - /drone141/Planning1/goal_point1 - - /drone141/Planning1/drone_path1 - - /drone141/Simulation1/robot1 - - /drone151/Planning1 - - /drone151/Planning1/goal_point1 - - /drone151/Planning1/optimal_traj1 - - /drone151/Planning1/drone_path1 - - /drone151/Simulation1 - - /drone151/Simulation1/robot1 - - /drone161/Planning1 - - /drone161/Planning1/goal_point1 - - /drone161/Planning1/optimal_traj1 - - /drone161/Planning1/drone_path1 - - /drone161/Simulation1 - - /drone161/Simulation1/robot1 - - /drone171/Planning1/goal_point1 - - /drone171/Planning1/optimal_traj1 - - /drone171/Planning1/drone_path1 - - /drone171/Mapping1 - - /drone171/Simulation1 - - /drone171/Simulation1/robot1 - - /drone181/Planning1 - - /drone181/Planning1/goal_point1 - - /drone181/Planning1/optimal_traj1 - - /drone181/Planning1/drone_path1 - - /drone181/Simulation1 - - /drone181/Simulation1/robot1 - - /drone191/Planning1 - - /drone191/Planning1/goal_point1 - - /drone191/Planning1/optimal_traj1 - - /drone191/Planning1/drone_path1 - - /drone191/Simulation1 - - /drone191/Simulation1/robot1 - - /drone201/Planning1 - - /drone201/Planning1/goal_point1 - - /drone201/Planning1/drone_path1 - - /drone201/Mapping1 - - /drone201/Simulation1 - - /drone201/Simulation1/robot1 - - /drone231/map inflate1 - - /drone241/map inflate1 - - /ugv31/path_cmd1 - - /ugv101/Marker1 - - /ugv151/lpcl1/Status1 - Splitter Ratio: 0.4421416223049164 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: simulation_map -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Class: rviz/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 0.15000000596046448 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 3 - Min Value: 0.009999999776482582 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 85; 170; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: simulation_map - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Boxes - Topic: /map_generator/global_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav1_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav1_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav1/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav1_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav1/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav1/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone1 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav2_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav2_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav2_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav2/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 115; 210; 22 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav2_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 115; 210; 22 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav2/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav2/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav2_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone2 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav3_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav3_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav3_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 0; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav3/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav3_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav3/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav3/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav3_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone3 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav4_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav4_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav4_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav4/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav4_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav4/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav4/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav4_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone4 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav5_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav5_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav5_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav5/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav5_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav5/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav5/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone5 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav6_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav6_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav6_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 190; 120; 121 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav6/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 190; 120; 121 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav6_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav6/prometheus/drone_mesh - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav6_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone6 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav7_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav7_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav7_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav7/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav7_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav7/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone7 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav8_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav8_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav8_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 226; 0; 158 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav8/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 226; 0; 158 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav8_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav8/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone8 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav9_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav9_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 85; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav9/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav9_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav9/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone9 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav10_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav10_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 29; 108; 212 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav10/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav10_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav10/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone10 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav11_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav11_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav11/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav11_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav11/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone11 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav12_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav12_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav12/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav12_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav12/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone12 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav13_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav13_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 0; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav13/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav13_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav13/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone13 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav14_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav14_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav14/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav14_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav14/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone14 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav15_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav15_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav15/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav15_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav15/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone15 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav16_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav16_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 190; 120; 121 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav16/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav16_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav16/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone16 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav17_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav17_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav17/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav17_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav17/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone17 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav18_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav18_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 226; 0; 158 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav18/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav18_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav18/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone18 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav19_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav19_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 170; 85; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav19/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav19_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav19/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone19 - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav20_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/global_list - Name: global_path - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav20_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /ego_planner_node/a_star_list - Name: AStar - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /uav9_ego_planner_node/init_list - Name: InitTraj - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 20; 106; 172 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav20/prometheus/drone_trajectory - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 85; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /uav20_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav20/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /uav5_pcl_render_node/depth - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: Simulation - Enabled: true - Name: drone20 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav21_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav21_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav21/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav21_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav21/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav21/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone21 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav22_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav22_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav22/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav22_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav22/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav22/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone22 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav23_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav23_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav23/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav23_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav23/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav23/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone23 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav24_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav24_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav24/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav24_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav24/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav24/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone24 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav25_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav25_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav25/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav25_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav25/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav25/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone25 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav26_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav26_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav26/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav26_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav26/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav26/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone26 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav27_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav27_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /av27/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav27_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav27/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav27/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone27 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav28_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav28_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav28/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav28_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav28/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav28/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone28 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav29_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav29_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav29/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav29_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav29/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav29/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone29 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav30_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav30_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav30/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav30_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav30/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav30/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone30 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav31_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav31_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav31/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav31_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav31/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav31/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone31 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav32_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav32_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav32/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav32_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav32/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav32/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone32 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav33_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav33_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav33/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav33_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav33/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav33/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone33 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav34_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav34_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav34/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav34_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav34/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav34/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone34 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav35_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav35_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav35/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav35_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav35/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav35/prometheus/drone_mesh - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - Enabled: true - Name: drone35 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav36_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav36_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav36/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav36_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav36/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav36/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone36 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav37_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav37_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav37/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav37_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav37/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav37/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone37 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav38_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav38_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav38/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav38_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav38/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav38/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone38 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav39_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav39_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav39/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav39_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav39/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav39/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone39 - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav40_ego_planner_node/goal_point - Name: goal_point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav40_ego_planner_node/optimal_list - Name: optimal_traj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 164; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /uav40/prometheus/drone_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: /uav40_ego_planner_node/grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: map_generator_local - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav40/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /uav40/prometheus/drone_mesh - Name: robot - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: drone40 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv1/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv1/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv1/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv1/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv1/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv1 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 173; 127; 168 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv2/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv2/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv2/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv2/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 252; 175; 62 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv2/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv2 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv3/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv3/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv3/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv3/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 255; 244 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv3/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv3 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv4/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv4/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv4/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv4/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 55; 44; 220 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv4/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv4 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv5/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv5/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv5/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv5/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 85; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv5/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv5 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv6/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv6/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv6/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv6/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 27; 244; 120 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv6/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv6 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv7/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv7/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv7/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv7/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 244; 4; 10 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv7/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv7 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv8/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv8/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv8/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv8/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 24; 4; 101 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv8/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv8 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv9/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv9/prometheus/ugv_mesh - Name: Marker - Namespaces: - ugv_mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv9/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv9/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 12; 220 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv9/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv9 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv10/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv10/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv10/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv10/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv10/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv10 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv11/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv11/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv11/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv11/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv11/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv11 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 173; 127; 168 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv12/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv12/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv12/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv12/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 252; 175; 62 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv12/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv12 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv13/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv13/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv13/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv13/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 255; 244 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /uav13/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv13 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv14/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv14/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv14/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv14/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 55; 44; 220 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv14/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv14 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv15/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv15/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv15/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv15/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 85; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv15/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv15 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv16/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv16/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv16/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv16/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 27; 244; 120 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv16/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv16 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv17/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv17/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv17/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv17/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 244; 4; 10 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv17/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv17 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv18/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv18/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv18/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv18/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 24; 4; 101 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv18/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv18 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv19/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv19/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv19/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv19/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 170; 12; 220 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv19/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv19 - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lpcl - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ugv20/map_generator/local_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /ugv20/prometheus/ugv_mesh - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 233; 79 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: path_cmd - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv20/prometheus/case2/path_cmd - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: traj - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /ugv20/prometheus/ugv_trajectory - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: gpcl_inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /ugv20/prometheus/planning/global_inflate_pcl - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: ugv20 - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 60.05042266845703 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -10.626058578491211 - Y: -1.4291949272155762 - Z: -1.1815851394203492e-5 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.424796462059021 - Target Frame: - Value: ThirdPersonFollower (rviz) - Yaw: 3.1253488063812256 - Saved: - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4000000059604645 - Position: - X: -11 - Y: 0 - Z: 8 - Target Frame: my_view - Value: FPS (rviz) - Yaw: 0 - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5 - Position: - X: -10 - Y: 0 - Z: 10 - Target Frame: my_view - Value: FPS (rviz) - Yaw: 0 - - Class: rviz/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: FPS - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Position: - X: -10 - Y: 0 - Z: 10 - Target Frame: my_view - Value: FPS (rviz) - Yaw: 0 -Window Geometry: - Displays: - collapsed: true - Height: 1383 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1879 - X: 681 - Y: 27 - depth: - collapsed: false diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_4uav.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_4uav.launch deleted file mode 100644 index 092b9c49..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_4uav.launch +++ /dev/null @@ -1,79 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_basic.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_basic.launch deleted file mode 100644 index ae8a927c..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_basic.launch +++ /dev/null @@ -1,69 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_pub_goal.launch b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_pub_goal.launch deleted file mode 100644 index 658bc875..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_pub_goal.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/package.xml b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/package.xml deleted file mode 100644 index c3776ea6..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/package.xml +++ /dev/null @@ -1,82 +0,0 @@ - - - ego_planner - 0.0.0 - The ego_planner package - - - - - iszhouxin - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - - plan_env - path_searching - bspline_opt - traj_utils - - roscpp - std_msgs - - roscpp - std_msgs - message_runtime - - plan_env - path_searching - bspline_opt - traj_utils - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_planner_node.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_planner_node.cpp deleted file mode 100644 index 21ef6c36..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_planner_node.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include -#include - -#include - -using namespace ego_planner; - -int main(int argc, char **argv) -{ - - ros::init(argc, argv, "ego_planner_node"); - ros::NodeHandle nh("~"); - - EGOReplanFSM rebo_replan; - - rebo_replan.init(nh); - - // ros::Duration(1.0).sleep(); - ros::spin(); - - return 0; -} - -// #include -// #include -// #include - -// #include - -// using namespace ego_planner; - -// void SignalHandler(int signal) { -// if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){ -// ros::shutdown(); -// } -// } - -// int main(int argc, char **argv) { - -// signal(SIGINT, SignalHandler); -// signal(SIGTERM,SignalHandler); - -// ros::init(argc, argv, "ego_planner_node", ros::init_options::NoSigintHandler); -// ros::NodeHandle nh("~"); - -// EGOReplanFSM rebo_replan; - -// rebo_replan.init(nh); - -// // ros::Duration(1.0).sleep(); -// ros::AsyncSpinner async_spinner(4); -// async_spinner.start(); -// ros::waitForShutdown(); - -// return 0; -// } \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_replan_fsm.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_replan_fsm.cpp deleted file mode 100644 index febb17ec..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_replan_fsm.cpp +++ /dev/null @@ -1,955 +0,0 @@ - -#include - -namespace ego_planner -{ - - void EGOReplanFSM::init(ros::NodeHandle &nh) - { - current_wp_ = 0; - exec_state_ = FSM_EXEC_STATE::INIT; - have_target_ = false; - have_odom_ = false; - have_recv_pre_agent_ = false; - - /* fsm param */ - // 目标点类型：1，手动设定目标点；2，预设目标点 - nh.param("fsm/flight_type", target_type_, -1); - // 重规划时间间隔 - nh.param("fsm/thresh_replan_time", replan_thresh_, -1.0); - // 与目标距离小于该参数时，停止规划 - nh.param("fsm/thresh_no_replan_meter", no_replan_thresh_, -1.0); - // 规划范围 - nh.param("fsm/planning_horizon", planning_horizen_, -1.0); - // 紧急停止时间 - nh.param("fsm/emergency_time", emergency_time_, 1.0); - // 仿真与实际标志位，实际中需要等待一个起始话题 - nh.param("fsm/realworld_experiment", flag_realworld_experiment_, false); - // 未知 - nh.param("fsm/fail_safe", enable_fail_safe_, true); - // 真实实验中需要等待"/traj_start_trigger"话题 - have_trigger_ = !flag_realworld_experiment_; - // 读取waypoint - nh.param("fsm/waypoint_num", waypoint_num_, -1); - for (int i = 0; i < waypoint_num_; i++) - { - nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0); - nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0); - nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0); - } - - /* initialize main modules */ - // 显示类 - visualization_.reset(new PlanningVisualization(nh)); - // 规划器类 - planner_manager_.reset(new EGOPlannerManager); - planner_manager_->initPlanModules(nh, visualization_); - planner_manager_->deliverTrajToOptimizer(); // store trajectories - planner_manager_->setDroneIdtoOpt(); - - /* callback */ - // 规划状态机定时器 - exec_timer_ = nh.createTimer(ros::Duration(0.01), &EGOReplanFSM::execFSMCallback, this); - // 安全检查定时器 - safety_timer_ = nh.createTimer(ros::Duration(0.05), &EGOReplanFSM::checkCollisionCallback, this); - // 订阅里程计 - odom_sub_ = nh.subscribe("odom_world", 1, &EGOReplanFSM::odometryCallback, this); - - // 订阅其他无人机位置 - // ego默认从0开始，我们默认从1开始，因此这里>2 - if (planner_manager_->pp_.drone_id >= 2) - { - string sub_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id - 1) + string("_planning/swarm_trajs"); - swarm_trajs_sub_ = nh.subscribe(sub_topic_name.c_str(), 10, &EGOReplanFSM::swarmTrajsCallback, this, ros::TransportHints().tcpNoDelay()); - } - string pub_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id) + string("_planning/swarm_trajs"); - swarm_trajs_pub_ = nh.advertise(pub_topic_name.c_str(), 10); - - broadcast_bspline_pub_ = nh.advertise("planning/broadcast_bspline_from_planner", 10); - broadcast_bspline_sub_ = nh.subscribe("planning/broadcast_bspline_to_planner", 100, &EGOReplanFSM::BroadcastBsplineCallback, this, ros::TransportHints().tcpNoDelay()); - - bspline_pub_ = nh.advertise("planning/bspline", 10); - data_disp_pub_ = nh.advertise("planning/data_display", 100); - - if (target_type_ == TARGET_TYPE::MANUAL_TARGET) - { - string waypoint_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id) + string("/prometheus/motion_planning/goal"); - waypoint_sub_ = nh.subscribe(waypoint_topic_name.c_str(), 1, &EGOReplanFSM::waypointCallback, this); - } - else if (target_type_ == TARGET_TYPE::PRESET_TARGET) - { - trigger_sub_ = nh.subscribe("/uav" + std::to_string(planner_manager_->pp_.drone_id) + "/traj_start_trigger", 1, &EGOReplanFSM::triggerCallback, this); - - ROS_INFO("Wait for 1 second."); - int count = 0; - while (ros::ok() && count++ < 1000) - { - ros::spinOnce(); - ros::Duration(0.001).sleep(); - } - - ROS_WARN("Waiting for trigger from [n3ctrl] from RC"); - - while (ros::ok() && (!have_odom_ || !have_trigger_)) - { - ros::spinOnce(); - ros::Duration(0.001).sleep(); - } - - readGivenWps(); - } - else - cout << "Wrong target_type_ value! target_type_=" << target_type_ << endl; - } - - void EGOReplanFSM::readGivenWps() - { - if (waypoint_num_ <= 0) - { - ROS_ERROR("Wrong waypoint_num_ = %d", waypoint_num_); - return; - } - - wps_.resize(waypoint_num_); - for (int i = 0; i < waypoint_num_; i++) - { - wps_[i](0) = waypoints_[i][0]; - wps_[i](1) = waypoints_[i][1]; - wps_[i](2) = waypoints_[i][2]; - } - - for (size_t i = 0; i < (size_t)waypoint_num_; i++) - { - // 发布目标点用于显示 "/drone_x_ego_planner_node/goal_point" - [目标点,颜色,大小,id] - visualization_->displayGoalPoint(wps_[i], Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i); - ros::Duration(0.001).sleep(); - } - - // 执行第一个路径点 - wp_id_ = 0; - planNextWaypoint(wps_[wp_id_]); - } - - void EGOReplanFSM::planNextWaypoint(const Eigen::Vector3d next_wp) - { - bool success = false; - // planGlobalTraj(起始位置\速度\加速度,终点位置\速度\加速度) - success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), next_wp, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); - - if (success) - { - end_pt_ = next_wp; - - constexpr double step_size_t = 0.1; - // planner_manager_->global_data_.global_duration_是整段轨迹的预估时间 - int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t); - vector gloabl_traj(i_end); - for (int i = 0; i < i_end; i++) - { - // 按照step_size_t提取路径点,按照时间分布 - gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t); - } - - end_vel_.setZero(); - have_target_ = true; - have_new_target_ = true; - - /*** FSM ***/ - if (exec_state_ == WAIT_TARGET) - changeFSMExecState(GEN_NEW_TRAJ, "TRIG"); - else - { - while (exec_state_ != EXEC_TRAJ) - { - ros::spinOnce(); - ros::Duration(0.001).sleep(); - } - changeFSMExecState(REPLAN_TRAJ, "TRIG"); - } - - // 发布GlobalPath用于显示 "/drone_x_ego_planner_node/global_list" - [GlobalPath,大小,id] - visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0); - } - else - { - ROS_ERROR("Unable to generate global trajectory!"); - } - } - - void EGOReplanFSM::triggerCallback(const geometry_msgs::PoseStampedPtr &msg) - { - have_trigger_ = true; - cout << "Triggered!" << endl; - init_pt_ = odom_pos_; - } - - void EGOReplanFSM::waypointCallback(const geometry_msgs::PoseStampedPtr &msg) - { - if (msg->pose.position.z < -0.1) - return; - - if(msg->pose.position.x == 99.99 && msg->pose.position.y == 99.99) - { - callEmergencyStop(odom_pos_); - have_target_ = false; - exec_state_ = WAIT_TARGET; - return; - } - - callEmergencyStop(odom_pos_); - sleep(2.0); - - cout << "EGO: Get goal!" << endl; - init_pt_ = odom_pos_; - - // 此处定高1米 - Eigen::Vector3d end_wp(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); - - // 发布目标点用于显示 - [目标点,颜色,大小,id] - visualization_->displayGoalPoint(end_wp, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 1); - - planNextWaypoint(end_wp); - } - - void EGOReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg) - { - odom_pos_(0) = msg->pose.pose.position.x; - odom_pos_(1) = msg->pose.pose.position.y; - odom_pos_(2) = msg->pose.pose.position.z; - - odom_vel_(0) = msg->twist.twist.linear.x; - odom_vel_(1) = msg->twist.twist.linear.y; - odom_vel_(2) = msg->twist.twist.linear.z; - - //odom_acc_ = estimateAcc( msg ); - - odom_orient_.w() = msg->pose.pose.orientation.w; - odom_orient_.x() = msg->pose.pose.orientation.x; - odom_orient_.y() = msg->pose.pose.orientation.y; - odom_orient_.z() = msg->pose.pose.orientation.z; - - have_odom_ = true; - } - - void EGOReplanFSM::BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg) - { - // printf("BroadcastBsplineCallback!\n"); - size_t id = msg->drone_id; - if ((int)id == planner_manager_->pp_.drone_id) - return; - - if (abs((ros::Time::now() - msg->start_time).toSec()) > 0.25) - { - ROS_ERROR("Time difference is too large! Local - Remote Agent %d = %fs", - msg->drone_id, (ros::Time::now() - msg->start_time).toSec()); - return; - } - - /* Fill up the buffer */ - if (planner_manager_->swarm_trajs_buf_.size() <= id) - { - for (size_t i = planner_manager_->swarm_trajs_buf_.size(); i <= id; i++) - { - OneTrajDataOfSwarm blank; - blank.drone_id = -1; - planner_manager_->swarm_trajs_buf_.push_back(blank); - } - } - - /* Test distance to the agent */ - Eigen::Vector3d cp0(msg->pos_pts[0].x, msg->pos_pts[0].y, msg->pos_pts[0].z); - Eigen::Vector3d cp1(msg->pos_pts[1].x, msg->pos_pts[1].y, msg->pos_pts[1].z); - Eigen::Vector3d cp2(msg->pos_pts[2].x, msg->pos_pts[2].y, msg->pos_pts[2].z); - Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6; - if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f) - { - planner_manager_->swarm_trajs_buf_[id].drone_id = -1; - return; // if the current drone is too far to the received agent. - } - - /* Store data */ - Eigen::MatrixXd pos_pts(3, msg->pos_pts.size()); - Eigen::VectorXd knots(msg->knots.size()); - for (size_t j = 0; j < msg->knots.size(); ++j) - { - knots(j) = msg->knots[j]; - } - for (size_t j = 0; j < msg->pos_pts.size(); ++j) - { - pos_pts(0, j) = msg->pos_pts[j].x; - pos_pts(1, j) = msg->pos_pts[j].y; - pos_pts(2, j) = msg->pos_pts[j].z; - } - - planner_manager_->swarm_trajs_buf_[id].drone_id = id; - - if (msg->order % 2) - { - double cutback = (double)msg->order / 2 + 1.5; - planner_manager_->swarm_trajs_buf_[id].duration_ = msg->knots[msg->knots.size() - ceil(cutback)]; - } - else - { - double cutback = (double)msg->order / 2 + 1.5; - planner_manager_->swarm_trajs_buf_[id].duration_ = (msg->knots[msg->knots.size() - floor(cutback)] + msg->knots[msg->knots.size() - ceil(cutback)]) / 2; - } - - UniformBspline pos_traj(pos_pts, msg->order, msg->knots[1] - msg->knots[0]); - pos_traj.setKnot(knots); - planner_manager_->swarm_trajs_buf_[id].position_traj_ = pos_traj; - - planner_manager_->swarm_trajs_buf_[id].start_pos_ = planner_manager_->swarm_trajs_buf_[id].position_traj_.evaluateDeBoorT(0); - - planner_manager_->swarm_trajs_buf_[id].start_time_ = msg->start_time; - // planner_manager_->swarm_trajs_buf_[id].start_time_ = ros::Time::now(); // Un-reliable time sync - - /* Check Collision */ - // printf("Check Collision\n"); - if (planner_manager_->checkCollision(id)) - { - changeFSMExecState(REPLAN_TRAJ, "TRAJ_CHECK"); - } - } - - void EGOReplanFSM::swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg) - { - - multi_bspline_msgs_buf_.traj.clear(); - multi_bspline_msgs_buf_ = *msg; - - // cout << "\033[45;33mmulti_bspline_msgs_buf.drone_id_from=" << multi_bspline_msgs_buf_.drone_id_from << " multi_bspline_msgs_buf_.traj.size()=" << multi_bspline_msgs_buf_.traj.size() << "\033[0m" << endl; - - if (!have_odom_) - { - ROS_ERROR("swarmTrajsCallback(): no odom!, return."); - return; - } - - if ((int)msg->traj.size() != msg->drone_id_from + 1) // drone_id must start from 0 - { - ROS_ERROR("Wrong trajectory size! msg->traj.size()=%d, msg->drone_id_from+1=%d", (int)msg->traj.size(), msg->drone_id_from + 1); - return; - } - - if (msg->traj[0].order != 3) // only support B-spline order equals 3. - { - ROS_ERROR("Only support B-spline order equals 3."); - return; - } - - // Step 1. receive the trajectories - planner_manager_->swarm_trajs_buf_.clear(); - planner_manager_->swarm_trajs_buf_.resize(msg->traj.size()); - - for (size_t i = 0; i < msg->traj.size(); i++) - { - - Eigen::Vector3d cp0(msg->traj[i].pos_pts[0].x, msg->traj[i].pos_pts[0].y, msg->traj[i].pos_pts[0].z); - Eigen::Vector3d cp1(msg->traj[i].pos_pts[1].x, msg->traj[i].pos_pts[1].y, msg->traj[i].pos_pts[1].z); - Eigen::Vector3d cp2(msg->traj[i].pos_pts[2].x, msg->traj[i].pos_pts[2].y, msg->traj[i].pos_pts[2].z); - Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6; - if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f) - { - planner_manager_->swarm_trajs_buf_[i].drone_id = -1; - continue; - } - - Eigen::MatrixXd pos_pts(3, msg->traj[i].pos_pts.size()); - Eigen::VectorXd knots(msg->traj[i].knots.size()); - for (size_t j = 0; j < msg->traj[i].knots.size(); ++j) - { - knots(j) = msg->traj[i].knots[j]; - } - for (size_t j = 0; j < msg->traj[i].pos_pts.size(); ++j) - { - pos_pts(0, j) = msg->traj[i].pos_pts[j].x; - pos_pts(1, j) = msg->traj[i].pos_pts[j].y; - pos_pts(2, j) = msg->traj[i].pos_pts[j].z; - } - - planner_manager_->swarm_trajs_buf_[i].drone_id = i; - - if (msg->traj[i].order % 2) - { - double cutback = (double)msg->traj[i].order / 2 + 1.5; - planner_manager_->swarm_trajs_buf_[i].duration_ = msg->traj[i].knots[msg->traj[i].knots.size() - ceil(cutback)]; - } - else - { - double cutback = (double)msg->traj[i].order / 2 + 1.5; - planner_manager_->swarm_trajs_buf_[i].duration_ = (msg->traj[i].knots[msg->traj[i].knots.size() - floor(cutback)] + msg->traj[i].knots[msg->traj[i].knots.size() - ceil(cutback)]) / 2; - } - - // planner_manager_->swarm_trajs_buf_[i].position_traj_ = - UniformBspline pos_traj(pos_pts, msg->traj[i].order, msg->traj[i].knots[1] - msg->traj[i].knots[0]); - pos_traj.setKnot(knots); - planner_manager_->swarm_trajs_buf_[i].position_traj_ = pos_traj; - - planner_manager_->swarm_trajs_buf_[i].start_pos_ = planner_manager_->swarm_trajs_buf_[i].position_traj_.evaluateDeBoorT(0); - - planner_manager_->swarm_trajs_buf_[i].start_time_ = msg->traj[i].start_time; - } - - have_recv_pre_agent_ = true; - - printf("have_recv_pre_agent_==true\n"); - } - - void EGOReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call) - { - - if (new_state == exec_state_) - continously_called_times_++; - else - continously_called_times_ = 1; - - static string state_str[8] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP", "SEQUENTIAL_START"}; - int pre_s = int(exec_state_); - exec_state_ = new_state; - // COMMENT - cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl; - } - - std::pair EGOReplanFSM::timesOfConsecutiveStateCalls() - { - return std::pair(continously_called_times_, exec_state_); - } - - void EGOReplanFSM::printFSMExecState() - { - static string state_str[8] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP", "SEQUENTIAL_START"}; - - cout << "/uav"<< planner_manager_->pp_.drone_id <<" planner state: " + state_str[int(exec_state_)] << endl; - } - - void EGOReplanFSM::execFSMCallback(const ros::TimerEvent &e) - { - // 暂停计时 - exec_timer_.stop(); // To avoid blockage (阻塞) - - static int fsm_num = 0; - fsm_num++; - if (fsm_num == 1000) - { - printFSMExecState(); - if (!have_odom_) - cout << "no odom." << endl; - if (!have_target_) - // cout << "wait for goal or trigger." << endl; - fsm_num = 0; - } - - switch (exec_state_) - { - case INIT: - { - if (!have_odom_) - { - goto force_return; - // return; - } - changeFSMExecState(WAIT_TARGET, "FSM"); - break; - } - - case WAIT_TARGET: - { - if (!have_target_ || !have_trigger_) - goto force_return; - // return; - else - { - changeFSMExecState(SEQUENTIAL_START, "FSM"); - } - break; - } - - // 按顺序启动 - case SEQUENTIAL_START: // for swarm - { - // swarmTrajsCallback回调后,have_recv_pre_agent_会被设置为true - // ego默认从0开始，我们默认从1开始，因此这里>2 - if (planner_manager_->pp_.drone_id <= 1 || (planner_manager_->pp_.drone_id >= 2&& have_recv_pre_agent_)) - { - if (have_odom_ && have_target_ && have_trigger_) - { - bool success = planFromGlobalTraj(10); // zx-todo - if (success) - { - changeFSMExecState(EXEC_TRAJ, "FSM"); - - publishSwarmTrajs(true); - } - else - { - ROS_ERROR("Failed to generate the first trajectory!!!"); - changeFSMExecState(SEQUENTIAL_START, "FSM"); - } - } - else - { - ROS_ERROR("No odom or no target! have_odom_=%d, have_target_=%d", have_odom_, have_target_); - } - } - - break; - } - - case GEN_NEW_TRAJ: - { - - // Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1); - // start_yaw_(0) = atan2(rot_x(1), rot_x(0)); - // start_yaw_(1) = start_yaw_(2) = 0.0; - - bool success = planFromGlobalTraj(10); // zx-todo - if (success) - { - changeFSMExecState(EXEC_TRAJ, "FSM"); - flag_escape_emergency_ = true; - publishSwarmTrajs(false); - } - else - { - changeFSMExecState(GEN_NEW_TRAJ, "FSM"); - } - break; - } - - case REPLAN_TRAJ: - { - - if (planFromCurrentTraj(1)) - { - changeFSMExecState(EXEC_TRAJ, "FSM"); - publishSwarmTrajs(false); - } - else - { - changeFSMExecState(REPLAN_TRAJ, "FSM"); - } - - break; - } - - case EXEC_TRAJ: - { - /* determine if need to replan */ - LocalTrajData *info = &planner_manager_->local_data_; - ros::Time time_now = ros::Time::now(); - double t_cur = (time_now - info->start_time_).toSec(); - t_cur = min(info->duration_, t_cur); - - // 当前位置从轨迹中读取,可改为从odom中读取 - Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur); - - /* && (end_pt_ - pos).norm() < 0.5 */ - if ((target_type_ == TARGET_TYPE::PRESET_TARGET) && - (wp_id_ < waypoint_num_ - 1) && - (end_pt_ - pos).norm() < no_replan_thresh_) - { - wp_id_++; - planNextWaypoint(wps_[wp_id_]); - } - else if ((local_target_pt_ - end_pt_).norm() < 1e-3) // end_pt_是目标点， - { - if (t_cur > info->duration_ - 1e-2) - { - have_target_ = false; - have_trigger_ = false; - - if (target_type_ == TARGET_TYPE::PRESET_TARGET) - { - wp_id_ = 0; - planNextWaypoint(wps_[wp_id_]); - } - - changeFSMExecState(WAIT_TARGET, "FSM"); - goto force_return; - // return; - } - else if ((end_pt_ - pos).norm() > no_replan_thresh_ && t_cur > replan_thresh_) - { - changeFSMExecState(REPLAN_TRAJ, "FSM"); - } - } - else if (t_cur > replan_thresh_) - { - changeFSMExecState(REPLAN_TRAJ, "FSM"); - } - - break; - } - - case EMERGENCY_STOP: - { - - if (flag_escape_emergency_) // Avoiding repeated calls - { - callEmergencyStop(odom_pos_); - } - else - { - if (enable_fail_safe_ && odom_vel_.norm() < 0.1) - changeFSMExecState(GEN_NEW_TRAJ, "FSM"); - } - - flag_escape_emergency_ = false; - break; - } - } - - data_disp_.header.stamp = ros::Time::now(); - data_disp_pub_.publish(data_disp_); - - force_return:; - exec_timer_.start(); - } - - bool EGOReplanFSM::planFromGlobalTraj(const int trial_times /*=1*/) //zx-todo - { - start_pt_ = odom_pos_; - start_vel_ = odom_vel_; - start_acc_.setZero(); - - bool flag_random_poly_init; - // 如果切换了exec状态,timesOfConsecutiveStateCalls().first=1 - if (timesOfConsecutiveStateCalls().first == 1) - flag_random_poly_init = false; - else - flag_random_poly_init = true; - - for (int i = 0; i < trial_times; i++) - { - if (callReboundReplan(true, flag_random_poly_init)) - { - return true; - } - } - return false; - } - - bool EGOReplanFSM::planFromCurrentTraj(const int trial_times /*=1*/) - { - - LocalTrajData *info = &planner_manager_->local_data_; - ros::Time time_now = ros::Time::now(); - double t_cur = (time_now - info->start_time_).toSec(); - - //cout << "info->velocity_traj_=" << info->velocity_traj_.get_control_points() << endl; - - // 将start_pt改为当前位置? - start_pt_ = info->position_traj_.evaluateDeBoorT(t_cur); - start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur); - start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur); - - bool success = callReboundReplan(false, false); - - if (!success) - { - success = callReboundReplan(true, false); - //changeFSMExecState(EXEC_TRAJ, "FSM"); - if (!success) - { - for (int i = 0; i < trial_times; i++) - { - success = callReboundReplan(true, true); - if (success) - break; - } - if (!success) - { - return false; - } - } - } - - return true; - } - - void EGOReplanFSM::checkCollisionCallback(const ros::TimerEvent &e) - { - - LocalTrajData *info = &planner_manager_->local_data_; - auto map = planner_manager_->grid_map_; - - if (exec_state_ == WAIT_TARGET || info->start_time_.toSec() < 1e-5) - return; - - /* ---------- check lost of depth ---------- */ - if(map->getOdomDepthTimeout()) - { - ROS_ERROR("Depth Lost! EMERGENCY_STOP"); - enable_fail_safe_ = false; - changeFSMExecState(EMERGENCY_STOP, "SAFETY"); - } - else - { - if(enable_fail_safe_ == false) - { - ROS_INFO("Depth Get! GEN_NEW_TRAJ"); - enable_fail_safe_ = true; - } - } - - /* ---------- check trajectory ---------- */ - constexpr double time_step = 0.01; - double t_cur = (ros::Time::now() - info->start_time_).toSec(); - Eigen::Vector3d p_cur = info->position_traj_.evaluateDeBoorT(t_cur); - const double CLEARANCE = 1.0 * planner_manager_->getSwarmClearance(); - double t_cur_global = ros::Time::now().toSec(); - double t_2_3 = info->duration_ * 2 / 3; - for (double t = t_cur; t < info->duration_; t += time_step) - { - if (t_cur < t_2_3 && t >= t_2_3) // If t_cur < t_2_3, only the first 2/3 partition of the trajectory is considered valid and will get checked. - break; - - bool occ = false; - occ |= map->getInflateOccupancy(info->position_traj_.evaluateDeBoorT(t)); - - for (size_t id = 0; id < planner_manager_->swarm_trajs_buf_.size(); id++) - { - if ((planner_manager_->swarm_trajs_buf_.at(id).drone_id != (int)id) || (planner_manager_->swarm_trajs_buf_.at(id).drone_id == planner_manager_->pp_.drone_id)) - { - continue; - } - - double t_X = t_cur_global - planner_manager_->swarm_trajs_buf_.at(id).start_time_.toSec(); - Eigen::Vector3d swarm_pridicted = planner_manager_->swarm_trajs_buf_.at(id).position_traj_.evaluateDeBoorT(t_X); - double dist = (p_cur - swarm_pridicted).norm(); - - if (dist < CLEARANCE) - { - occ = true; - break; - } - } - - if (occ) - { - - if (planFromCurrentTraj()) // Make a chance - { - changeFSMExecState(EXEC_TRAJ, "SAFETY"); - publishSwarmTrajs(false); - return; - } - else - { - if (t - t_cur < emergency_time_) // 0.8s of emergency time - { - ROS_WARN("Suddenly discovered obstacles. emergency stop! time=%f", t - t_cur); - changeFSMExecState(EMERGENCY_STOP, "SAFETY"); - } - else - { - //ROS_WARN("current traj in collision, replan."); - changeFSMExecState(REPLAN_TRAJ, "SAFETY"); - } - return; - } - break; - } - } - } - - bool EGOReplanFSM::callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj) - { - // 根据planning_horizen_来确定局部目标点 local_target_pt_,local_target_vel_ - getLocalTarget(); - - // - bool plan_and_refine_success = - planner_manager_->reboundReplan(start_pt_, start_vel_, start_acc_, local_target_pt_, local_target_vel_, (have_new_target_ || flag_use_poly_init), flag_randomPolyTraj); - have_new_target_ = false; - - // comment - // cout << "refine_success=" << plan_and_refine_success << endl; - - if (plan_and_refine_success) - { - - auto info = &planner_manager_->local_data_; - - traj_utils::Bspline bspline; - bspline.order = 3; - bspline.start_time = info->start_time_; - bspline.traj_id = info->traj_id_; - - Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint(); - bspline.pos_pts.reserve(pos_pts.cols()); - for (int i = 0; i < pos_pts.cols(); ++i) - { - geometry_msgs::Point pt; - pt.x = pos_pts(0, i); - pt.y = pos_pts(1, i); - pt.z = pos_pts(2, i); - bspline.pos_pts.push_back(pt); - } - - Eigen::VectorXd knots = info->position_traj_.getKnot(); - // cout << knots.transpose() << endl; - bspline.knots.reserve(knots.rows()); - for (int i = 0; i < knots.rows(); ++i) - { - bspline.knots.push_back(knots(i)); - } - - /* 1. publish traj to traj_server */ - bspline_pub_.publish(bspline); - - /* 2. publish traj to the next drone of swarm */ - - /* 3. publish traj for visualization */ - // 发布优化后的轨迹 "/optimal_list" , 颜色和scale已经内置 - visualization_->displayOptimalList(info->position_traj_.get_control_points(), 0); - } - - return plan_and_refine_success; - } - - void EGOReplanFSM::publishSwarmTrajs(bool startup_pub) - { - auto info = &planner_manager_->local_data_; - - traj_utils::Bspline bspline; - bspline.order = 3; - bspline.start_time = info->start_time_; - bspline.drone_id = planner_manager_->pp_.drone_id; - bspline.traj_id = info->traj_id_; - - Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint(); - bspline.pos_pts.reserve(pos_pts.cols()); - for (int i = 0; i < pos_pts.cols(); ++i) - { - geometry_msgs::Point pt; - pt.x = pos_pts(0, i); - pt.y = pos_pts(1, i); - pt.z = pos_pts(2, i); - bspline.pos_pts.push_back(pt); - } - - Eigen::VectorXd knots = info->position_traj_.getKnot(); - // cout << knots.transpose() << endl; - bspline.knots.reserve(knots.rows()); - for (int i = 0; i < knots.rows(); ++i) - { - bspline.knots.push_back(knots(i)); - } - - if (startup_pub) - { - multi_bspline_msgs_buf_.drone_id_from = planner_manager_->pp_.drone_id; // zx-todo - if ((int)multi_bspline_msgs_buf_.traj.size() == planner_manager_->pp_.drone_id + 1) - { - multi_bspline_msgs_buf_.traj.back() = bspline; - } - else if ((int)multi_bspline_msgs_buf_.traj.size() == planner_manager_->pp_.drone_id) - { - multi_bspline_msgs_buf_.traj.push_back(bspline); - } - else - { - ROS_ERROR("Wrong traj nums and drone_id pair!!! traj.size()=%d, drone_id=%d", (int)multi_bspline_msgs_buf_.traj.size(), planner_manager_->pp_.drone_id); - // return plan_and_refine_success; - } - swarm_trajs_pub_.publish(multi_bspline_msgs_buf_); - } - - broadcast_bspline_pub_.publish(bspline); - } - - bool EGOReplanFSM::callEmergencyStop(Eigen::Vector3d stop_pos) - { - - planner_manager_->EmergencyStop(stop_pos); - - auto info = &planner_manager_->local_data_; - - /* publish traj */ - traj_utils::Bspline bspline; - bspline.order = 3; - bspline.start_time = info->start_time_; - bspline.traj_id = info->traj_id_; - - Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint(); - bspline.pos_pts.reserve(pos_pts.cols()); - for (int i = 0; i < pos_pts.cols(); ++i) - { - geometry_msgs::Point pt; - pt.x = pos_pts(0, i); - pt.y = pos_pts(1, i); - pt.z = pos_pts(2, i); - bspline.pos_pts.push_back(pt); - } - - Eigen::VectorXd knots = info->position_traj_.getKnot(); - bspline.knots.reserve(knots.rows()); - for (int i = 0; i < knots.rows(); ++i) - { - bspline.knots.push_back(knots(i)); - } - - bspline_pub_.publish(bspline); - - return true; - } - - void EGOReplanFSM::getLocalTarget() - { - double t; - - double t_step = planning_horizen_ / 20 / planner_manager_->pp_.max_vel_; - double dist_min = 9999, dist_min_t = 0.0; - for (t = planner_manager_->global_data_.last_progress_time_; t < planner_manager_->global_data_.global_duration_; t += t_step) - { - Eigen::Vector3d pos_t = planner_manager_->global_data_.getPosition(t); - double dist = (pos_t - start_pt_).norm(); - - if (t < planner_manager_->global_data_.last_progress_time_ + 1e-5 && dist > planning_horizen_) - { - // Important conor case! - for (; t < planner_manager_->global_data_.global_duration_; t += t_step) - { - Eigen::Vector3d pos_t_temp = planner_manager_->global_data_.getPosition(t); - double dist_temp = (pos_t_temp - start_pt_).norm(); - if (dist_temp < planning_horizen_) - { - pos_t = pos_t_temp; - dist = (pos_t - start_pt_).norm(); - cout << "Escape conor case \"getLocalTarget\"" << endl; - break; - } - } - } - - if (dist < dist_min) - { - dist_min = dist; - dist_min_t = t; - } - - if (dist >= planning_horizen_) - { - local_target_pt_ = pos_t; - planner_manager_->global_data_.last_progress_time_ = dist_min_t; - break; - } - } - if (t > planner_manager_->global_data_.global_duration_) // Last global point - { - local_target_pt_ = end_pt_; - planner_manager_->global_data_.last_progress_time_ = planner_manager_->global_data_.global_duration_; - } - - if ((end_pt_ - local_target_pt_).norm() < (planner_manager_->pp_.max_vel_ * planner_manager_->pp_.max_vel_) / (2 * planner_manager_->pp_.max_acc_)) - { - local_target_vel_ = Eigen::Vector3d::Zero(); - } - else - { - local_target_vel_ = planner_manager_->global_data_.getVelocity(t); - } - } - -} // namespace ego_planner diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_traj_to_cmd.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_traj_to_cmd.cpp deleted file mode 100644 index 1dd8e29b..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/ego_traj_to_cmd.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "prometheus_msgs/DroneState.h" -#include "prometheus_msgs/SwarmCommand.h" -#include "quadrotor_msgs/PositionCommand.h" -#include "std_msgs/Empty.h" -#include "uav_utils/geometry_utils.h" -#include "printf_utils.h" - -// 无人机名字 -string uav_name; -// 无人机编号 -int uav_id; -int control_flag; -// ego输出 -quadrotor_msgs::PositionCommand ego_traj_cmd; -bool get_ego_traj; -// 发布的控制指令 -prometheus_msgs::SwarmCommand Command_Now; - -ros::Subscriber ego_ouput_sub; -ros::Publisher command_pub; - -void ego_ouput_cb(const quadrotor_msgs::PositionCommand::ConstPtr& msg) -{ - ego_traj_cmd = *msg; - get_ego_traj = true; - - if(ego_traj_cmd.velocity.x == 0) - { - get_ego_traj = false; - // cout << YELLOW << " Arrive the goal " << TAIL <>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< -int main(int argc, char **argv) -{ - ros::init(argc, argv, "ego_traj_to_cmd"); - ros::NodeHandle nh("~"); - ros::Rate rate(100.0); - - // 无人机编号 1号无人机则为1 - nh.param("uav_id", uav_id, 0); - nh.param("control_flag", control_flag, 0); - - uav_name = "/uav" + std::to_string(uav_id); - - // 【订阅】EGO的轨迹输出(traj_server的输出) - ego_ouput_sub = nh.subscribe(uav_name + "/prometheus/ego/traj_cmd", 1, ego_ouput_cb); - - // 【发布】 路径指令 （发送至swarm_controller.cpp） - command_pub = nh.advertise(uav_name + "/prometheus/swarm_command", 1); - - while(ros::ok()) - { - ros::spinOnce(); - rate.sleep(); - } -} - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/planner_manager.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/planner_manager.cpp deleted file mode 100644 index bd09892d..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/planner_manager.cpp +++ /dev/null @@ -1,600 +0,0 @@ -// #include -#include -#include -#include "visualization_msgs/Marker.h" // zx-todo - -namespace ego_planner -{ - - // SECTION interfaces for setup and query - - EGOPlannerManager::EGOPlannerManager() {} - - EGOPlannerManager::~EGOPlannerManager() {} - - void EGOPlannerManager::initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis) - { - /* read algorithm parameters */ - // 最大速度 - nh.param("manager/max_vel", pp_.max_vel_, -1.0); - // 最大加速度 - nh.param("manager/max_acc", pp_.max_acc_, -1.0); - // 最大加加速度 - nh.param("manager/max_jerk", pp_.max_jerk_, -1.0); - // 未知，限制参数 - nh.param("manager/feasibility_tolerance", pp_.feasibility_tolerance_, 0.0); - // 控制点距离 - nh.param("manager/control_points_distance", pp_.ctrl_pt_dist, -1.0); - // 规划范围 - nh.param("manager/planning_horizon", pp_.planning_horizen_, 5.0); - // ？ - nh.param("manager/use_distinctive_trajs", pp_.use_distinctive_trajs, false); - // 无人机ID - nh.param("manager/drone_id", pp_.drone_id, 0); - - local_data_.traj_id_ = 0; - // 地图类 - grid_map_.reset(new GridMap); - grid_map_->initMap(nh); - - // 默认没有使用 - // obj_predictor_.reset(new fast_planner::ObjPredictor(nh)); - // obj_predictor_->init(); - // obj_pub_ = nh.advertise("/dynamic/obj_prdi", 10); // zx-todo - - // bspline优化器 - bspline_optimizer_.reset(new BsplineOptimizer); - bspline_optimizer_->setParam(nh); - bspline_optimizer_->setEnvironment(grid_map_, obj_predictor_); - bspline_optimizer_->a_star_.reset(new AStar); - bspline_optimizer_->a_star_->initGridMap(grid_map_, Eigen::Vector3i(100, 100, 100)); - - visualization_ = vis; - } - - // !SECTION - - // SECTION rebond replanning - - bool EGOPlannerManager::reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, - Eigen::Vector3d start_acc, Eigen::Vector3d local_target_pt, - Eigen::Vector3d local_target_vel, bool flag_polyInit, bool flag_randomPolyTraj) - { - static int count = 0; - // comment - // printf("\033[47;30m\n[drone no.%d , replan %d times]====================================\033[0m\n", pp_.drone_id, count++); - // cout.precision(3); - // cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << "\ngoal:" << local_target_pt.transpose() << ", " << local_target_vel.transpose() - // << endl; - - if ((start_pt - local_target_pt).norm() < 0.2) - { - cout << "The start point is too Close to goal" << endl; - continous_failures_count_++; - return false; - } - - bspline_optimizer_->setLocalTargetPt(local_target_pt); - - ros::Time t_start = ros::Time::now(); - ros::Duration t_init, t_opt, t_refine; - - /*** STEP 1: INIT ***/ - double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.5 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5; // pp_.ctrl_pt_dist / pp_.max_vel_ is too tense, and will surely exceed the acc/vel limits - vector point_set, start_end_derivatives; - static bool flag_first_call = true, flag_force_polynomial = false; - bool flag_regenerate = false; - do - { - point_set.clear(); - start_end_derivatives.clear(); - flag_regenerate = false; - - if (flag_first_call || flag_polyInit || flag_force_polynomial /*|| ( start_pt - local_target_pt ).norm() < 1.0*/) // Initial path generated from a min-snap traj by order. - { - flag_first_call = false; - flag_force_polynomial = false; - - PolynomialTraj gl_traj; - - double dist = (start_pt - local_target_pt).norm(); - double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_; - - if (!flag_randomPolyTraj) - { - gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc, local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), time); - } - else - { - Eigen::Vector3d horizen_dir = ((start_pt - local_target_pt).cross(Eigen::Vector3d(0, 0, 1))).normalized(); - Eigen::Vector3d vertical_dir = ((start_pt - local_target_pt).cross(horizen_dir)).normalized(); - Eigen::Vector3d random_inserted_pt = (start_pt + local_target_pt) / 2 + - (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * horizen_dir * 0.8 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989) + - (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * vertical_dir * 0.4 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989); - Eigen::MatrixXd pos(3, 3); - pos.col(0) = start_pt; - pos.col(1) = random_inserted_pt; - pos.col(2) = local_target_pt; - Eigen::VectorXd t(2); - t(0) = t(1) = time / 2; - gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, local_target_vel, start_acc, Eigen::Vector3d::Zero(), t); - } - - double t; - bool flag_too_far; - ts *= 1.5; // ts will be divided by 1.5 in the next - do - { - ts /= 1.5; - point_set.clear(); - flag_too_far = false; - Eigen::Vector3d last_pt = gl_traj.evaluate(0); - for (t = 0; t < time; t += ts) - { - Eigen::Vector3d pt = gl_traj.evaluate(t); - if ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5) - { - flag_too_far = true; - break; - } - last_pt = pt; - point_set.push_back(pt); - } - } while (flag_too_far || point_set.size() < 7); // To make sure the initial path has enough points. - t -= ts; - start_end_derivatives.push_back(gl_traj.evaluateVel(0)); - start_end_derivatives.push_back(local_target_vel); - start_end_derivatives.push_back(gl_traj.evaluateAcc(0)); - start_end_derivatives.push_back(gl_traj.evaluateAcc(t)); - } - else // Initial path generated from previous trajectory. - { - - double t; - double t_cur = (ros::Time::now() - local_data_.start_time_).toSec(); - - vector pseudo_arc_length; - vector segment_point; - pseudo_arc_length.push_back(0.0); - for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts) - { - segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t)); - if (t > t_cur) - { - pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back()); - } - } - t -= ts; - - double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2; - if (poly_time > ts) - { - PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t), - local_data_.velocity_traj_.evaluateDeBoorT(t), - local_data_.acceleration_traj_.evaluateDeBoorT(t), - local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), poly_time); - - for (t = ts; t < poly_time; t += ts) - { - if (!pseudo_arc_length.empty()) - { - segment_point.push_back(gl_traj.evaluate(t)); - pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back()); - } - else - { - ROS_ERROR("pseudo_arc_length is empty, return!"); - continous_failures_count_++; - return false; - } - } - } - - double sample_length = 0; - double cps_dist = pp_.ctrl_pt_dist * 1.5; // cps_dist will be divided by 1.5 in the next - size_t id = 0; - do - { - cps_dist /= 1.5; - point_set.clear(); - sample_length = 0; - id = 0; - while ((id <= pseudo_arc_length.size() - 2) && sample_length <= pseudo_arc_length.back()) - { - if (sample_length >= pseudo_arc_length[id] && sample_length < pseudo_arc_length[id + 1]) - { - point_set.push_back((sample_length - pseudo_arc_length[id]) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id + 1] + - (pseudo_arc_length[id + 1] - sample_length) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id]); - sample_length += cps_dist; - } - else - id++; - } - point_set.push_back(local_target_pt); - } while (point_set.size() < 7); // If the start point is very close to end point, this will help - - start_end_derivatives.push_back(local_data_.velocity_traj_.evaluateDeBoorT(t_cur)); - start_end_derivatives.push_back(local_target_vel); - start_end_derivatives.push_back(local_data_.acceleration_traj_.evaluateDeBoorT(t_cur)); - start_end_derivatives.push_back(Eigen::Vector3d::Zero()); - - if (point_set.size() > pp_.planning_horizen_ / pp_.ctrl_pt_dist * 3) // The initial path is unnormally too long! - { - flag_force_polynomial = true; - flag_regenerate = true; - } - } - } while (flag_regenerate); - - Eigen::MatrixXd ctrl_pts, ctrl_pts_temp; - UniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts); - - vector> segments; - segments = bspline_optimizer_->initControlPoints(ctrl_pts, true); - - t_init = ros::Time::now() - t_start; - t_start = ros::Time::now(); - - /*** STEP 2: OPTIMIZE ***/ - bool flag_step_1_success = false; - vector> vis_trajs; - - if (pp_.use_distinctive_trajs) - { - // cout << "enter" << endl; - std::vector trajs = bspline_optimizer_->distinctiveTrajs(segments); - // comment - // cout << "\033[1;33m"<< "multi-trajs=" << trajs.size() << "\033[1;0m" << endl; - - double final_cost, min_cost = 999999.0; - for (int i = trajs.size() - 1; i >= 0; i--) - { - if (bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts_temp, final_cost, trajs[i], ts)) - { - //comment - // cout << "traj " << trajs.size() - i << " success." << endl; - - flag_step_1_success = true; - if (final_cost < min_cost) - { - min_cost = final_cost; - ctrl_pts = ctrl_pts_temp; - } - - // visualization - point_set.clear(); - for (int j = 0; j < ctrl_pts_temp.cols(); j++) - { - point_set.push_back(ctrl_pts_temp.col(j)); - } - vis_trajs.push_back(point_set); - } - else - { - cout << "traj " << trajs.size() - i << " failed." << endl; - } - } - - t_opt = ros::Time::now() - t_start; - - // 显示 "/init_list" - visualization_->displayMultiInitPathList(vis_trajs, 0.2); // This visuallization will take up several milliseconds. - } - else - { - flag_step_1_success = bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts, ts); - t_opt = ros::Time::now() - t_start; - //static int vis_id = 0; - visualization_->displayInitPathList(point_set, 0.2, 0); - } - // comment - // cout << "/uav"<< pp_.drone_id<< " plan_success=" << flag_step_1_success << endl; - if (!flag_step_1_success) - { - visualization_->displayOptimalList(ctrl_pts, 0); - continous_failures_count_++; - return false; - } - - t_start = ros::Time::now(); - - UniformBspline pos = UniformBspline(ctrl_pts, 3, ts); - pos.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_, pp_.feasibility_tolerance_); - - /*** STEP 3: REFINE(RE-ALLOCATE TIME) IF NECESSARY ***/ - // Note: Only adjust time in single drone mode. But we still allow drone_0 to adjust its time profile. - // ego默认从0开始，我们默认从1开始，因此这里<=1 - if (pp_.drone_id <= 1) - { - - double ratio; - bool flag_step_2_success = true; - if (!pos.checkFeasibility(ratio, false)) - { - cout << "Need to reallocate time." << endl; - - Eigen::MatrixXd optimal_control_points; - flag_step_2_success = refineTrajAlgo(pos, start_end_derivatives, ratio, ts, optimal_control_points); - if (flag_step_2_success) - pos = UniformBspline(optimal_control_points, 3, ts); - } - - if (!flag_step_2_success) - { - printf("\033[34mThis refined trajectory hits obstacles. It doesn't matter if appeares occasionally. But if continously appearing, Increase parameter \"lambda_fitness\".\n\033[0m"); - continous_failures_count_++; - return false; - } - } - else - { - static bool print_once = true; - if (print_once) - { - print_once = false; - ROS_ERROR("IN SWARM MODE, REFINE DISABLED!"); - } - } - - t_refine = ros::Time::now() - t_start; - - // save planned results - updateTrajInfo(pos, ros::Time::now()); - - static double sum_time = 0; - static int count_success = 0; - sum_time += (t_init + t_opt + t_refine).toSec(); - count_success++; - // comment - // cout << "total time:\033[42m" << (t_init + t_opt + t_refine).toSec() << "\033[0m,optimize:" << (t_init + t_opt).toSec() << ",refine:" << t_refine.toSec() << ",avg_time=" << sum_time / count_success << endl; - - // success. YoY - continous_failures_count_ = 0; - return true; - } - - bool EGOPlannerManager::EmergencyStop(Eigen::Vector3d stop_pos) - { - Eigen::MatrixXd control_points(3, 6); - for (int i = 0; i < 6; i++) - { - control_points.col(i) = stop_pos; - } - - updateTrajInfo(UniformBspline(control_points, 3, 1.0), ros::Time::now()); - - return true; - } - - bool EGOPlannerManager::checkCollision(int drone_id) - { - if (local_data_.start_time_.toSec() < 1e9) // It means my first planning has not started - return false; - - double my_traj_start_time = local_data_.start_time_.toSec(); - double other_traj_start_time = swarm_trajs_buf_[drone_id].start_time_.toSec(); - - double t_start = max(my_traj_start_time, other_traj_start_time); - double t_end = min(my_traj_start_time + local_data_.duration_ * 2 / 3, other_traj_start_time + swarm_trajs_buf_[drone_id].duration_); - - for (double t = t_start; t < t_end; t += 0.03) - { - if ((local_data_.position_traj_.evaluateDeBoorT(t - my_traj_start_time) - swarm_trajs_buf_[drone_id].position_traj_.evaluateDeBoorT(t - other_traj_start_time)).norm() < bspline_optimizer_->getSwarmClearance()) - { - return true; - } - } - - return false; - } - - bool EGOPlannerManager::planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, - const std::vector &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc) - { - - // generate global reference trajectory - - vector points; - points.push_back(start_pos); - - for (size_t wp_i = 0; wp_i < waypoints.size(); wp_i++) - { - points.push_back(waypoints[wp_i]); - } - - double total_len = 0; - total_len += (start_pos - waypoints[0]).norm(); - for (size_t i = 0; i < waypoints.size() - 1; i++) - { - total_len += (waypoints[i + 1] - waypoints[i]).norm(); - } - - // insert intermediate points if too far - vector inter_points; - double dist_thresh = max(total_len / 8, 4.0); - - for (size_t i = 0; i < points.size() - 1; ++i) - { - inter_points.push_back(points.at(i)); - double dist = (points.at(i + 1) - points.at(i)).norm(); - - if (dist > dist_thresh) - { - int id_num = floor(dist / dist_thresh) + 1; - - for (int j = 1; j < id_num; ++j) - { - Eigen::Vector3d inter_pt = - points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num; - inter_points.push_back(inter_pt); - } - } - } - - inter_points.push_back(points.back()); - - // for ( int i=0; i= 3) - gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time); - else if (pos.cols() == 2) - gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc, pos.col(1), end_vel, end_acc, time(0)); - else - return false; - - auto time_now = ros::Time::now(); - global_data_.setGlobalTraj(gl_traj, time_now); - - return true; - } - - bool EGOPlannerManager::planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc) - { - - // generate global reference trajectory,生成全局参考轨迹 - vector points; - points.push_back(start_pos); - points.push_back(end_pos); - - // insert intermediate points if too far - vector inter_points; - const double dist_thresh = 4.0; - - // points.size() = 2, 这个for循环只执行一次? - for (size_t i = 0; i < points.size() - 1; ++i) - { - // push_back初始点 - inter_points.push_back(points.at(i)); - // 目标点与起始点的距离 - double dist = (points.at(i + 1) - points.at(i)).norm(); - - // 距离大于4米 - if (dist > dist_thresh) - { - // 以4米为一个单位计算增加的点数 - int id_num = floor(dist / dist_thresh) + 1; - - for (int j = 1; j < id_num; ++j) - { - Eigen::Vector3d inter_pt = - points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num; - inter_points.push_back(inter_pt); - } - } - } - - // push_back目标点 - inter_points.push_back(points.back()); - - // write position matrix - int pt_num = inter_points.size(); - // 航点矩阵 - Eigen::MatrixXd pos(3, pt_num); - // 赋值 - for (int i = 0; i < pt_num; ++i) - pos.col(i) = inter_points[i]; - - Eigen::Vector3d zero(0, 0, 0); - Eigen::VectorXd time(pt_num - 1); - for (int i = 0; i < pt_num - 1; ++i) - { - // 预估时间 - time(i) = (pos.col(i + 1) - pos.col(i)).norm() / (pp_.max_vel_); - } - - // 第一段时间扩大2倍 - time(0) *= 2.0; - // 最后一段时间扩大2倍 - time(time.rows() - 1) *= 2.0; - - PolynomialTraj gl_traj; - // 如果仅有一段,或有多段 - if (pos.cols() >= 3) - gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time); - else if (pos.cols() == 2) - gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc, end_pos, end_vel, end_acc, time(0)); - else - return false; - - auto time_now = ros::Time::now(); - // 存入global_data_,等待下一步处理 - global_data_.setGlobalTraj(gl_traj, time_now); - - return true; - } - - bool EGOPlannerManager::refineTrajAlgo(UniformBspline &traj, vector &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points) - { - double t_inc; - - Eigen::MatrixXd ctrl_pts; // = traj.getControlPoint() - - // std::cout << "ratio: " << ratio << std::endl; - reparamBspline(traj, start_end_derivative, ratio, ctrl_pts, ts, t_inc); - - traj = UniformBspline(ctrl_pts, 3, ts); - - double t_step = traj.getTimeSum() / (ctrl_pts.cols() - 3); - bspline_optimizer_->ref_pts_.clear(); - for (double t = 0; t < traj.getTimeSum() + 1e-4; t += t_step) - bspline_optimizer_->ref_pts_.push_back(traj.evaluateDeBoorT(t)); - - bool success = bspline_optimizer_->BsplineOptimizeTrajRefine(ctrl_pts, ts, optimal_control_points); - - return success; - } - - void EGOPlannerManager::updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now) - { - local_data_.start_time_ = time_now; - local_data_.position_traj_ = position_traj; - local_data_.velocity_traj_ = local_data_.position_traj_.getDerivative(); - local_data_.acceleration_traj_ = local_data_.velocity_traj_.getDerivative(); - local_data_.start_pos_ = local_data_.position_traj_.evaluateDeBoorT(0.0); - local_data_.duration_ = local_data_.position_traj_.getTimeSum(); - local_data_.traj_id_ += 1; - } - - void EGOPlannerManager::reparamBspline(UniformBspline &bspline, vector &start_end_derivative, double ratio, - Eigen::MatrixXd &ctrl_pts, double &dt, double &time_inc) - { - double time_origin = bspline.getTimeSum(); - int seg_num = bspline.getControlPoint().cols() - 3; - // double length = bspline.getLength(0.1); - // int seg_num = ceil(length / pp_.ctrl_pt_dist); - - bspline.lengthenTime(ratio); - double duration = bspline.getTimeSum(); - dt = duration / double(seg_num); - time_inc = duration - time_origin; - - vector point_set; - for (double time = 0.0; time <= duration + 1e-4; time += dt) - { - point_set.push_back(bspline.evaluateDeBoorT(time)); - } - UniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts); - } - -} // namespace ego_planner diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/traj_server.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/traj_server.cpp deleted file mode 100644 index 92d68fdd..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src/traj_server.cpp +++ /dev/null @@ -1,264 +0,0 @@ -#include "bspline_opt/uniform_bspline.h" -#include "nav_msgs/Odometry.h" -#include "traj_utils/Bspline.h" -#include "quadrotor_msgs/PositionCommand.h" -#include "std_msgs/Empty.h" -#include "visualization_msgs/Marker.h" -#include - -ros::Publisher pos_cmd_pub; - -quadrotor_msgs::PositionCommand cmd; -double pos_gain[3] = {0, 0, 0}; -double vel_gain[3] = {0, 0, 0}; - -using ego_planner::UniformBspline; - -bool receive_traj_ = false; -vector traj_; -double traj_duration_; -ros::Time start_time_; -int traj_id_; - -// yaw control -double last_yaw_, last_yaw_dot_; -double time_forward_; - -void bsplineCallback(traj_utils::BsplineConstPtr msg) -{ - // parse pos traj - - Eigen::MatrixXd pos_pts(3, msg->pos_pts.size()); - - Eigen::VectorXd knots(msg->knots.size()); - for (size_t i = 0; i < msg->knots.size(); ++i) - { - knots(i) = msg->knots[i]; - } - - for (size_t i = 0; i < msg->pos_pts.size(); ++i) - { - pos_pts(0, i) = msg->pos_pts[i].x; - pos_pts(1, i) = msg->pos_pts[i].y; - pos_pts(2, i) = msg->pos_pts[i].z; - } - - UniformBspline pos_traj(pos_pts, msg->order, 0.1); - pos_traj.setKnot(knots); - - // parse yaw traj - - // Eigen::MatrixXd yaw_pts(msg->yaw_pts.size(), 1); - // for (int i = 0; i < msg->yaw_pts.size(); ++i) { - // yaw_pts(i, 0) = msg->yaw_pts[i]; - // } - - //UniformBspline yaw_traj(yaw_pts, msg->order, msg->yaw_dt); - - start_time_ = msg->start_time; - traj_id_ = msg->traj_id; - - traj_.clear(); - traj_.push_back(pos_traj); - traj_.push_back(traj_[0].getDerivative()); - traj_.push_back(traj_[1].getDerivative()); - - traj_duration_ = traj_[0].getTimeSum(); - - receive_traj_ = true; -} - -std::pair calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last) -{ - constexpr double PI = 3.1415926; - constexpr double YAW_DOT_MAX_PER_SEC = PI; - // constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI; - std::pair yaw_yawdot(0, 0); - double yaw = 0; - double yawdot = 0; - - Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos; - double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_; - double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec(); - if (yaw_temp - last_yaw_ > PI) - { - if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change) - { - yaw = last_yaw_ - max_yaw_change; - if (yaw < -PI) - yaw += 2 * PI; - - yawdot = -YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ > PI) - yawdot = -YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - else if (yaw_temp - last_yaw_ < -PI) - { - if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change) - { - yaw = last_yaw_ + max_yaw_change; - if (yaw > PI) - yaw -= 2 * PI; - - yawdot = YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ < -PI) - yawdot = YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - else - { - if (yaw_temp - last_yaw_ < -max_yaw_change) - { - yaw = last_yaw_ - max_yaw_change; - if (yaw < -PI) - yaw += 2 * PI; - - yawdot = -YAW_DOT_MAX_PER_SEC; - } - else if (yaw_temp - last_yaw_ > max_yaw_change) - { - yaw = last_yaw_ + max_yaw_change; - if (yaw > PI) - yaw -= 2 * PI; - - yawdot = YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ > PI) - yawdot = -YAW_DOT_MAX_PER_SEC; - else if (yaw - last_yaw_ < -PI) - yawdot = YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - - if (fabs(yaw - last_yaw_) <= max_yaw_change) - yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF - yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot; - last_yaw_ = yaw; - last_yaw_dot_ = yawdot; - - yaw_yawdot.first = yaw; - yaw_yawdot.second = yawdot; - - return yaw_yawdot; -} - -void cmdCallback(const ros::TimerEvent &e) -{ - /* no publishing before receive traj_ */ - if (!receive_traj_) - return; - - ros::Time time_now = ros::Time::now(); - double t_cur = (time_now - start_time_).toSec(); - - Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()), pos_f; - std::pair yaw_yawdot(0, 0); - - static ros::Time time_last = ros::Time::now(); - if (t_cur < traj_duration_ && t_cur >= 0.0) - { - pos = traj_[0].evaluateDeBoorT(t_cur); - vel = traj_[1].evaluateDeBoorT(t_cur); - acc = traj_[2].evaluateDeBoorT(t_cur); - - /*** calculate yaw ***/ - yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last); - /*** calculate yaw ***/ - - double tf = min(traj_duration_, t_cur + 2.0); - pos_f = traj_[0].evaluateDeBoorT(tf); - } - else if (t_cur >= traj_duration_) - { - /* hover when finish traj_ */ - pos = traj_[0].evaluateDeBoorT(traj_duration_); - vel.setZero(); - acc.setZero(); - - yaw_yawdot.first = last_yaw_; - yaw_yawdot.second = 0; - - pos_f = pos; - } - else - { - cout << "[Traj server]: invalid time." << endl; - } - time_last = time_now; - - cmd.header.stamp = time_now; - cmd.header.frame_id = "world"; - cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY; - cmd.trajectory_id = traj_id_; - - cmd.position.x = pos(0); - cmd.position.y = pos(1); - cmd.position.z = pos(2); - - cmd.velocity.x = vel(0); - cmd.velocity.y = vel(1); - cmd.velocity.z = vel(2); - - cmd.acceleration.x = acc(0); - cmd.acceleration.y = acc(1); - cmd.acceleration.z = acc(2); - - cmd.yaw = yaw_yawdot.first; - cmd.yaw_dot = yaw_yawdot.second; - - last_yaw_ = cmd.yaw; - - pos_cmd_pub.publish(cmd); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "traj_server"); - // ros::NodeHandle node; - ros::NodeHandle nh("~"); - - ros::Subscriber bspline_sub = nh.subscribe("planning/bspline", 10, bsplineCallback); - - pos_cmd_pub = nh.advertise("/position_cmd", 50); - - ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback); - - /* control parameter */ - cmd.kx[0] = pos_gain[0]; - cmd.kx[1] = pos_gain[1]; - cmd.kx[2] = pos_gain[2]; - - cmd.kv[0] = vel_gain[0]; - cmd.kv[1] = vel_gain[1]; - cmd.kv[2] = vel_gain[2]; - - nh.param("traj_server/time_forward", time_forward_, -1.0); - nh.param("traj_server/last_yaw", last_yaw_, 0.0); - last_yaw_dot_ = 0.0; - - ros::Duration(1.0).sleep(); - - ROS_WARN("[Traj server]: ready."); - - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/ego_goal_pub.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/ego_goal_pub.cpp deleted file mode 100644 index 894ac802..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/ego_goal_pub.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include - -#include -#include "printf_utils.h" - -#define MAX_SWRAM_NUM 41 -ros::Publisher planner_goal_pub[MAX_SWRAM_NUM]; -ros::Publisher planner_start_pub[MAX_SWRAM_NUM]; - -double target[10][3]; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "ego_goal_pub"); - ros::NodeHandle nh("~"); - - int swarm_num; - bool sim_mode; - // 无人机编号 1号无人机则为1 - nh.param("swarm_num", swarm_num, 1); - nh.param("sim_mode", sim_mode, true); - - string uav_name; - for (int i = 1; i <= swarm_num; i++) - { - nh.param("uav" + to_string(i) + "/target_x", target[i][0], -1.0); - nh.param("uav" + to_string(i) + "/target_y", target[i][1], -1.0); - nh.param("uav" + to_string(i) + "/target_z", target[i][2], -1.0); - - cout << GREEN << "uav_"<< i <<"/target: [" << target[i][0] << "," << target[i][1] << "," << target[i][2] << "]" << TAIL << endl; - - uav_name = "/uav" + std::to_string(i); - // 【发布】目标点至EGO-planner-swarm - planner_goal_pub[i] = nh.advertise(uav_name + "/prometheus/motion_planning/goal", 1); - // 【发布】start_trigger - planner_start_pub[i] = nh.advertise(uav_name + "/traj_start_trigger", 1); - } - - // 【订阅】EGO的轨迹输出(traj_server的输出) - // ego_ouput_sub = nh.subscribe(uav_name + "/prometheus/ego/traj_cmd", 1, &Case3FSM::ego_ouput_cb, this); - - bool flag; - cout << GREEN << "input 1 to pub target:" << TAIL << endl; - cin >> flag; - - geometry_msgs::PoseStamped target_point; - geometry_msgs::PoseStamped start_trigger; - for (int i = 1; i <= swarm_num; i++) - { - target_point.pose.position.x = target[i][0]; - target_point.pose.position.y = target[i][1]; - target_point.pose.position.z = target[i][2]; - - planner_goal_pub[i].publish(target_point); - // planner_start_pub[i].publish(start_trigger); - } - - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/traj_server_for_prometheus.cpp b/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/traj_server_for_prometheus.cpp deleted file mode 100644 index d8e537b9..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/traj_server_for_prometheus.cpp +++ /dev/null @@ -1,310 +0,0 @@ -#include -#include "bspline_opt/uniform_bspline.h" -#include "nav_msgs/Odometry.h" -#include "traj_utils/Bspline.h" -#include "quadrotor_msgs/PositionCommand.h" -#include -#include "std_msgs/Empty.h" -#include "visualization_msgs/Marker.h" - -#include "geometry_utils.h" - -// 无人机编号 -int uav_id; -int control_flag; - -ros::Publisher pos_cmd_pub, uav_cmd_pub; - -quadrotor_msgs::PositionCommand cmd; -double pos_gain[3] = {0, 0, 0}; -double vel_gain[3] = {0, 0, 0}; - -using ego_planner::UniformBspline; - -bool receive_traj_ = false; -vector traj_; -double traj_duration_; -ros::Time start_time_; -int traj_id_; - -// yaw control -double last_yaw_, last_yaw_dot_; -double time_forward_; - -void pub_prometheus_command(quadrotor_msgs::PositionCommand ego_traj_cmd); -void bsplineCallback(traj_utils::BsplineConstPtr msg); -void cmdCallback(const ros::TimerEvent &e); -std::pair calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last); - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "traj_server_for_prometheus"); - ros::NodeHandle nh("~"); - - // 无人机编号 1号无人机则为1 - nh.param("uav_id", uav_id, 0); - nh.param("control_flag", control_flag, 0); - nh.param("traj_server/time_forward", time_forward_, 1.0); - nh.param("traj_server/last_yaw", last_yaw_, 0.0); - string uav_name = "/uav" + std::to_string(uav_id); - - // [订阅] EGO规划结果 - ros::Subscriber bspline_sub = nh.subscribe(uav_name +"/planning/bspline", 10, bsplineCallback); - - // [发布] EGO规划结果 - pos_cmd_pub = nh.advertise(uav_name + "/position_cmd", 50); - - // [订阅] EGO规划结果 - to Prometheus - uav_cmd_pub = nh.advertise(uav_name + "/prometheus/command", 50); - - // [定时器] EGO规划结果发布定时器 - ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback); - - /* control parameter */ - cmd.kx[0] = pos_gain[0]; - cmd.kx[1] = pos_gain[1]; - cmd.kx[2] = pos_gain[2]; - - cmd.kv[0] = vel_gain[0]; - cmd.kv[1] = vel_gain[1]; - cmd.kv[2] = vel_gain[2]; - - last_yaw_dot_ = 0.0; - - ros::Duration(1.0).sleep(); - - ROS_WARN("[Traj server]: ready."); - - ros::spin(); - - return 0; -} - -void pub_prometheus_command(quadrotor_msgs::PositionCommand ego_traj_cmd) -{ - prometheus_msgs::UAVCommand uav_command; - uav_command.header.stamp = ros::Time::now(); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Command_ID = uav_command.Command_ID + 1; - if (control_flag == 0) - { - // pos_controller - uav_command.Move_mode = prometheus_msgs::UAVCommand::TRAJECTORY; - } - else if (control_flag == 1) - { - // px4_sender - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - } - uav_command.position_ref[0] = ego_traj_cmd.position.x; - uav_command.position_ref[1] = ego_traj_cmd.position.y; - uav_command.position_ref[2] = ego_traj_cmd.position.z; - uav_command.velocity_ref[0] = ego_traj_cmd.velocity.x; - uav_command.velocity_ref[1] = ego_traj_cmd.velocity.y; - uav_command.velocity_ref[2] = ego_traj_cmd.velocity.z; - uav_command.acceleration_ref[0] = ego_traj_cmd.acceleration.x; - uav_command.acceleration_ref[1] = ego_traj_cmd.acceleration.y; - uav_command.acceleration_ref[2] = ego_traj_cmd.acceleration.z; - uav_command.yaw_ref = geometry_utils::normalize_angle(ego_traj_cmd.yaw); - // uav_command.yaw_rate_ref = ego_traj_cmd.yaw_dot; - - uav_cmd_pub.publish(uav_command); -} - -void cmdCallback(const ros::TimerEvent &e) -{ - /* no publishing before receive traj_ */ - if (!receive_traj_) - return; - - ros::Time time_now = ros::Time::now(); - double t_cur = (time_now - start_time_).toSec(); - - Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()), pos_f; - std::pair yaw_yawdot(0, 0); - - static ros::Time time_last = ros::Time::now(); - if (t_cur < traj_duration_ && t_cur >= 0.0) - { - pos = traj_[0].evaluateDeBoorT(t_cur); - vel = traj_[1].evaluateDeBoorT(t_cur); - acc = traj_[2].evaluateDeBoorT(t_cur); - - /*** calculate yaw ***/ - yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last); - /*** calculate yaw ***/ - - double tf = min(traj_duration_, t_cur + 2.0); - pos_f = traj_[0].evaluateDeBoorT(tf); - } - else if (t_cur >= traj_duration_) - { - /* hover when finish traj_ */ - pos = traj_[0].evaluateDeBoorT(traj_duration_); - vel.setZero(); - acc.setZero(); - - yaw_yawdot.first = last_yaw_; - yaw_yawdot.second = 0; - - pos_f = pos; - } - else - { - cout << "[Traj server]: invalid time." << endl; - } - time_last = time_now; - - cmd.header.stamp = time_now; - cmd.header.frame_id = "world"; - cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY; - cmd.trajectory_id = traj_id_; - - cmd.position.x = pos(0); - cmd.position.y = pos(1); - cmd.position.z = pos(2); - - cmd.velocity.x = vel(0); - cmd.velocity.y = vel(1); - cmd.velocity.z = vel(2); - - cmd.acceleration.x = acc(0); - cmd.acceleration.y = acc(1); - cmd.acceleration.z = acc(2); - - cmd.yaw = yaw_yawdot.first; - cmd.yaw_dot = yaw_yawdot.second; - - last_yaw_ = cmd.yaw; - - pos_cmd_pub.publish(cmd); - - pub_prometheus_command(cmd); -} - -void bsplineCallback(traj_utils::BsplineConstPtr msg) -{ - // parse pos traj - - Eigen::MatrixXd pos_pts(3, msg->pos_pts.size()); - - Eigen::VectorXd knots(msg->knots.size()); - for (size_t i = 0; i < msg->knots.size(); ++i) - { - knots(i) = msg->knots[i]; - } - - for (size_t i = 0; i < msg->pos_pts.size(); ++i) - { - pos_pts(0, i) = msg->pos_pts[i].x; - pos_pts(1, i) = msg->pos_pts[i].y; - pos_pts(2, i) = msg->pos_pts[i].z; - } - - UniformBspline pos_traj(pos_pts, msg->order, 0.1); - pos_traj.setKnot(knots); - - start_time_ = msg->start_time; - traj_id_ = msg->traj_id; - - traj_.clear(); - traj_.push_back(pos_traj); - traj_.push_back(traj_[0].getDerivative()); - traj_.push_back(traj_[1].getDerivative()); - - traj_duration_ = traj_[0].getTimeSum(); - - receive_traj_ = true; -} - -std::pair calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last) -{ - constexpr double PI = 3.1415926; - constexpr double YAW_DOT_MAX_PER_SEC = PI; - // constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI; - std::pair yaw_yawdot(0, 0); - double yaw = 0; - double yawdot = 0; - - Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos; - double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_; - double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec(); - if (yaw_temp - last_yaw_ > PI) - { - if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change) - { - yaw = last_yaw_ - max_yaw_change; - if (yaw < -PI) - yaw += 2 * PI; - - yawdot = -YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ > PI) - yawdot = -YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - else if (yaw_temp - last_yaw_ < -PI) - { - if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change) - { - yaw = last_yaw_ + max_yaw_change; - if (yaw > PI) - yaw -= 2 * PI; - - yawdot = YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ < -PI) - yawdot = YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - else - { - if (yaw_temp - last_yaw_ < -max_yaw_change) - { - yaw = last_yaw_ - max_yaw_change; - if (yaw < -PI) - yaw += 2 * PI; - - yawdot = -YAW_DOT_MAX_PER_SEC; - } - else if (yaw_temp - last_yaw_ > max_yaw_change) - { - yaw = last_yaw_ + max_yaw_change; - if (yaw > PI) - yaw -= 2 * PI; - - yawdot = YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ > PI) - yawdot = -YAW_DOT_MAX_PER_SEC; - else if (yaw - last_yaw_ < -PI) - yawdot = YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - - if (fabs(yaw - last_yaw_) <= max_yaw_change) - yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF - yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot; - last_yaw_ = yaw; - last_yaw_dot_ = yawdot; - - yaw_yawdot.first = yaw; - yaw_yawdot.second = yawdot; - - return yaw_yawdot; -} diff --git a/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/CMakeLists.txt b/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/CMakeLists.txt deleted file mode 100644 index 30ec4192..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rosmsg_tcp_bridge) - -set(CMAKE_BUILD_TYPE "Release") -ADD_COMPILE_OPTIONS(-std=c++11 ) -ADD_COMPILE_OPTIONS(-std=c++14 ) -set(CMAKE_CXX_FLAGS_RELEASE "-O2 -Wall -g") - -find_package(Eigen3 REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - geometry_msgs - traj_utils -) - -catkin_package( - CATKIN_DEPENDS traj_utils -) - -include_directories( - SYSTEM - ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include - ${EIGEN3_INCLUDE_DIR} -) - -add_executable(bridge_node - src/bridge_node.cpp - ) -target_link_libraries(bridge_node - ${catkin_LIBRARIES} - ) - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/launch/bridge.launch b/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/launch/bridge.launch deleted file mode 100644 index babaa016..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/launch/bridge.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/package.xml b/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/package.xml deleted file mode 100644 index 677652f7..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/package.xml +++ /dev/null @@ -1,67 +0,0 @@ - - - rosmsg_tcp_bridge - 0.0.0 - The rosmsg_tcp_bridge package - - - - - iszhouxin - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - traj_utils - - roscpp - std_msgs - traj_utils - - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/src/bridge_node.cpp b/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/src/bridge_node.cpp deleted file mode 100644 index 04d7e304..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/rosmsg_tcp_bridge/src/bridge_node.cpp +++ /dev/null @@ -1,828 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#define PORT 8080 -#define UDP_PORT 8081 -#define BUF_LEN 1048576 // 1MB -#define BUF_LEN_SHORT 1024 // 1KB - -using namespace std; - -int send_sock_, server_fd_, recv_sock_, udp_server_fd_, udp_send_fd_; -ros::Subscriber swarm_trajs_sub_, other_odoms_sub_, emergency_stop_sub_, one_traj_sub_; -ros::Publisher swarm_trajs_pub_, other_odoms_pub_, emergency_stop_pub_, one_traj_pub_; -string tcp_ip_, udp_ip_; -int drone_id_; -double odom_broadcast_freq_; -char send_buf_[BUF_LEN], recv_buf_[BUF_LEN], udp_recv_buf_[BUF_LEN], udp_send_buf_[BUF_LEN]; -struct sockaddr_in addr_udp_send_; -traj_utils::MultiBsplinesPtr bsplines_msg_; -nav_msgs::OdometryPtr odom_msg_; -std_msgs::EmptyPtr stop_msg_; -traj_utils::BsplinePtr bspline_msg_; - -enum MESSAGE_TYPE -{ - ODOM = 888, - MULTI_TRAJ, - ONE_TRAJ, - STOP -} massage_type_; - -int connect_to_next_drone(const char *ip, const int port) -{ - /* Connect */ - int sock = 0; - struct sockaddr_in serv_addr; - if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) - { - printf("\n Socket creation error \n"); - return -1; - } - - serv_addr.sin_family = AF_INET; - serv_addr.sin_port = htons(port); - - // Convert IPv4 and IPv6 addresses from text to binary form - if (inet_pton(AF_INET, ip, &serv_addr.sin_addr) <= 0) - { - printf("\nInvalid address/ Address not supported \n"); - return -1; - } - - if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) - { - ROS_WARN("Tcp connection to drone_%d Failed", drone_id_+1); - return -1; - } - - char str[INET_ADDRSTRLEN]; - ROS_INFO("Connect to %s success!", inet_ntop(AF_INET, &serv_addr.sin_addr, str, sizeof(str))); - - return sock; -} - -int init_broadcast(const char *ip, const int port) -{ - int fd; - - if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) <= 0) - { - ROS_ERROR("[bridge_node]Socket sender creation error!"); - exit(EXIT_FAILURE); - } - - int so_broadcast = 1; - if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &so_broadcast, sizeof(so_broadcast)) < 0) - { - cout << "Error in setting Broadcast option"; - exit(EXIT_FAILURE); - } - - addr_udp_send_.sin_family = AF_INET; - addr_udp_send_.sin_port = htons(port); - - if (inet_pton(AF_INET, ip, &addr_udp_send_.sin_addr) <= 0) - { - printf("\nInvalid address/ Address not supported \n"); - return -1; - } - - return fd; -} - -int wait_connection_from_previous_drone(const int port, int &server_fd, int &new_socket) -{ - struct sockaddr_in address; - int opt = 1; - int addrlen = sizeof(address); - - // Creating socket file descriptor - if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) - { - perror("socket failed"); - exit(EXIT_FAILURE); - } - - // Forcefully attaching socket to the port - if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, - &opt, sizeof(opt))) - { - perror("setsockopt"); - exit(EXIT_FAILURE); - } - address.sin_family = AF_INET; - address.sin_addr.s_addr = INADDR_ANY; - address.sin_port = htons(PORT); - - // Forcefully attaching socket to the port - if (bind(server_fd, (struct sockaddr *)&address, - sizeof(address)) < 0) - { - perror("bind failed"); - exit(EXIT_FAILURE); - } - if (listen(server_fd, 3) < 0) - { - perror("listen"); - exit(EXIT_FAILURE); - } - if ((new_socket = accept(server_fd, (struct sockaddr *)&address, - (socklen_t *)&addrlen)) < 0) - { - perror("accept"); - exit(EXIT_FAILURE); - } - - char str[INET_ADDRSTRLEN]; - ROS_INFO( "Receive tcp connection from %s", inet_ntop(AF_INET, &address.sin_addr, str, sizeof(str)) ); - - return new_socket; -} - -int udp_bind_to_port(const int port, int &server_fd) -{ - struct sockaddr_in address; - int opt = 1; - - // Creating socket file descriptor - if ((server_fd = socket(AF_INET, SOCK_DGRAM, 0)) == 0) - { - perror("socket failed"); - exit(EXIT_FAILURE); - } - - // Forcefully attaching socket to the port - if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, - &opt, sizeof(opt))) - { - perror("setsockopt"); - exit(EXIT_FAILURE); - } - address.sin_family = AF_INET; - address.sin_addr.s_addr = INADDR_ANY; - address.sin_port = htons(port); - - // Forcefully attaching socket to the port - if (bind(server_fd, (struct sockaddr *)&address, - sizeof(address)) < 0) - { - perror("bind failed"); - exit(EXIT_FAILURE); - } - - return server_fd; -} - -int serializeMultiBsplines(const traj_utils::MultiBsplinesPtr &msg) -{ - char *ptr = send_buf_; - - unsigned long total_len = 0; - total_len += sizeof(MESSAGE_TYPE) + sizeof(int32_t) + sizeof(size_t); - for (size_t i = 0; i < msg->traj.size(); i++) - { - total_len += sizeof(int32_t) + sizeof(int32_t) + sizeof(double) + sizeof(int64_t) + sizeof(double); - total_len += sizeof(size_t) + msg->traj[i].knots.size() * sizeof(double); - total_len += sizeof(size_t) + (3 * msg->traj[i].pos_pts.size()) * sizeof(double); - total_len += sizeof(size_t) + msg->traj[i].yaw_pts.size() * sizeof(double); - } - if (total_len + 1 > BUF_LEN) - { - ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN"); - return -1; - } - - *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::MULTI_TRAJ; - ptr += sizeof(MESSAGE_TYPE); - - *((int32_t *)ptr) = msg->drone_id_from; - ptr += sizeof(int32_t); - if (ptr - send_buf_ > BUF_LEN) - { - } - *((size_t *)ptr) = msg->traj.size(); - ptr += sizeof(size_t); - for (size_t i = 0; i < msg->traj.size(); i++) - { - *((int32_t *)ptr) = msg->traj[i].drone_id; - ptr += sizeof(int32_t); - *((int32_t *)ptr) = msg->traj[i].order; - ptr += sizeof(int32_t); - *((double *)ptr) = msg->traj[i].start_time.toSec(); - ptr += sizeof(double); - *((int64_t *)ptr) = msg->traj[i].traj_id; - ptr += sizeof(int64_t); - *((double *)ptr) = msg->traj[i].yaw_dt; - ptr += sizeof(double); - - *((size_t *)ptr) = msg->traj[i].knots.size(); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->traj[i].knots.size(); j++) - { - *((double *)ptr) = msg->traj[i].knots[j]; - ptr += sizeof(double); - } - - *((size_t *)ptr) = msg->traj[i].pos_pts.size(); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->traj[i].pos_pts.size(); j++) - { - *((double *)ptr) = msg->traj[i].pos_pts[j].x; - ptr += sizeof(double); - *((double *)ptr) = msg->traj[i].pos_pts[j].y; - ptr += sizeof(double); - *((double *)ptr) = msg->traj[i].pos_pts[j].z; - ptr += sizeof(double); - } - - *((size_t *)ptr) = msg->traj[i].yaw_pts.size(); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->traj[i].yaw_pts.size(); j++) - { - *((double *)ptr) = msg->traj[i].yaw_pts[j]; - ptr += sizeof(double); - } - } - - return ptr - send_buf_; -} - -int serializeOdom(const nav_msgs::OdometryPtr &msg) -{ - char *ptr = udp_send_buf_; - - unsigned long total_len = 0; - total_len = sizeof(size_t) + - msg->child_frame_id.length() * sizeof(char) + - sizeof(size_t) + - msg->header.frame_id.length() * sizeof(char) + - sizeof(uint32_t) + - sizeof(double) + - 7 * sizeof(double) + - 36 * sizeof(double) + - 6 * sizeof(double) + - 36 * sizeof(double); - - if (total_len + 1 > BUF_LEN) - { - ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN"); - return -1; - } - - *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::ODOM; - ptr += sizeof(MESSAGE_TYPE); - - // child_frame_id - size_t len = msg->child_frame_id.length(); - *((size_t *)ptr) = len; - ptr += sizeof(size_t); - memcpy((void *)ptr, (void *)msg->child_frame_id.c_str(), len * sizeof(char)); - ptr += len * sizeof(char); - - // header - len = msg->header.frame_id.length(); - *((size_t *)ptr) = len; - ptr += sizeof(size_t); - memcpy((void *)ptr, (void *)msg->header.frame_id.c_str(), len * sizeof(char)); - ptr += len * sizeof(char); - *((uint32_t *)ptr) = msg->header.seq; - ptr += sizeof(uint32_t); - *((double *)ptr) = msg->header.stamp.toSec(); - ptr += sizeof(double); - - *((double *)ptr) = msg->pose.pose.position.x; - ptr += sizeof(double); - *((double *)ptr) = msg->pose.pose.position.y; - ptr += sizeof(double); - *((double *)ptr) = msg->pose.pose.position.z; - ptr += sizeof(double); - - *((double *)ptr) = msg->pose.pose.orientation.w; - ptr += sizeof(double); - *((double *)ptr) = msg->pose.pose.orientation.x; - ptr += sizeof(double); - *((double *)ptr) = msg->pose.pose.orientation.y; - ptr += sizeof(double); - *((double *)ptr) = msg->pose.pose.orientation.z; - ptr += sizeof(double); - - for (size_t j = 0; j < 36; j++) - { - *((double *)ptr) = msg->pose.covariance[j]; - ptr += sizeof(double); - } - - *((double *)ptr) = msg->twist.twist.linear.x; - ptr += sizeof(double); - *((double *)ptr) = msg->twist.twist.linear.y; - ptr += sizeof(double); - *((double *)ptr) = msg->twist.twist.linear.z; - ptr += sizeof(double); - *((double *)ptr) = msg->twist.twist.angular.x; - ptr += sizeof(double); - *((double *)ptr) = msg->twist.twist.angular.y; - ptr += sizeof(double); - *((double *)ptr) = msg->twist.twist.angular.z; - ptr += sizeof(double); - - for (size_t j = 0; j < 36; j++) - { - *((double *)ptr) = msg->twist.covariance[j]; - ptr += sizeof(double); - } - - return ptr - udp_send_buf_; -} - -int serializeStop(const std_msgs::EmptyPtr &msg) -{ - char *ptr = udp_send_buf_; - - *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::STOP; - ptr += sizeof(MESSAGE_TYPE); - - return ptr - udp_send_buf_; -} - -int serializeOneTraj(const traj_utils::BsplinePtr &msg) -{ - char *ptr = udp_send_buf_; - - unsigned long total_len = 0; - total_len += sizeof(int32_t) + sizeof(int32_t) + sizeof(double) + sizeof(int64_t) + sizeof(double); - total_len += sizeof(size_t) + msg->knots.size() * sizeof(double); - total_len += sizeof(size_t) + (3 * msg->pos_pts.size()) * sizeof(double); - total_len += sizeof(size_t) + msg->yaw_pts.size() * sizeof(double); - if (total_len + 1 > BUF_LEN) - { - ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN (2)"); - return -1; - } - - *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::ONE_TRAJ; - ptr += sizeof(MESSAGE_TYPE); - - *((int32_t *)ptr) = msg->drone_id; - ptr += sizeof(int32_t); - *((int32_t *)ptr) = msg->order; - ptr += sizeof(int32_t); - *((double *)ptr) = msg->start_time.toSec(); - ptr += sizeof(double); - *((int64_t *)ptr) = msg->traj_id; - ptr += sizeof(int64_t); - *((double *)ptr) = msg->yaw_dt; - ptr += sizeof(double); - - *((size_t *)ptr) = msg->knots.size(); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->knots.size(); j++) - { - *((double *)ptr) = msg->knots[j]; - ptr += sizeof(double); - } - - *((size_t *)ptr) = msg->pos_pts.size(); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->pos_pts.size(); j++) - { - *((double *)ptr) = msg->pos_pts[j].x; - ptr += sizeof(double); - *((double *)ptr) = msg->pos_pts[j].y; - ptr += sizeof(double); - *((double *)ptr) = msg->pos_pts[j].z; - ptr += sizeof(double); - } - - *((size_t *)ptr) = msg->yaw_pts.size(); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->yaw_pts.size(); j++) - { - *((double *)ptr) = msg->yaw_pts[j]; - ptr += sizeof(double); - } - - return ptr - udp_send_buf_; -} - -int deserializeOneTraj(traj_utils::BsplinePtr &msg) -{ - char *ptr = udp_recv_buf_; - - ptr += sizeof(MESSAGE_TYPE); - - msg->drone_id = *((int32_t *)ptr); - ptr += sizeof(int32_t); - msg->order = *((int32_t *)ptr); - ptr += sizeof(int32_t); - msg->start_time.fromSec(*((double *)ptr)); - ptr += sizeof(double); - msg->traj_id = *((int64_t *)ptr); - ptr += sizeof(int64_t); - msg->yaw_dt = *((double *)ptr); - ptr += sizeof(double); - msg->knots.resize(*((size_t *)ptr)); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->knots.size(); j++) - { - msg->knots[j] = *((double *)ptr); - ptr += sizeof(double); - } - - msg->pos_pts.resize(*((size_t *)ptr)); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->pos_pts.size(); j++) - { - msg->pos_pts[j].x = *((double *)ptr); - ptr += sizeof(double); - msg->pos_pts[j].y = *((double *)ptr); - ptr += sizeof(double); - msg->pos_pts[j].z = *((double *)ptr); - ptr += sizeof(double); - } - - msg->yaw_pts.resize(*((size_t *)ptr)); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->yaw_pts.size(); j++) - { - msg->yaw_pts[j] = *((double *)ptr); - ptr += sizeof(double); - } - - return ptr - udp_recv_buf_; -} - -int deserializeStop(std_msgs::EmptyPtr &msg) -{ - char *ptr = udp_recv_buf_; - - return ptr - udp_recv_buf_; -} - -int deserializeOdom(nav_msgs::OdometryPtr &msg) -{ - char *ptr = udp_recv_buf_; - - ptr += sizeof(MESSAGE_TYPE); - - // child_frame_id - size_t len = *((size_t *)ptr); - ptr += sizeof(size_t); - msg->child_frame_id.assign((const char *)ptr, len); - ptr += len * sizeof(char); - - // header - len = *((size_t *)ptr); - ptr += sizeof(size_t); - msg->header.frame_id.assign((const char *)ptr, len); - ptr += len * sizeof(char); - msg->header.seq = *((uint32_t *)ptr); - ptr += sizeof(uint32_t); - msg->header.stamp.fromSec(*((double *)ptr)); - ptr += sizeof(double); - - msg->pose.pose.position.x = *((double *)ptr); - ptr += sizeof(double); - msg->pose.pose.position.y = *((double *)ptr); - ptr += sizeof(double); - msg->pose.pose.position.z = *((double *)ptr); - ptr += sizeof(double); - - msg->pose.pose.orientation.w = *((double *)ptr); - ptr += sizeof(double); - msg->pose.pose.orientation.x = *((double *)ptr); - ptr += sizeof(double); - msg->pose.pose.orientation.y = *((double *)ptr); - ptr += sizeof(double); - msg->pose.pose.orientation.z = *((double *)ptr); - ptr += sizeof(double); - - for (size_t j = 0; j < 36; j++) - { - msg->pose.covariance[j] = *((double *)ptr); - ptr += sizeof(double); - } - - msg->twist.twist.linear.x = *((double *)ptr); - ptr += sizeof(double); - msg->twist.twist.linear.y = *((double *)ptr); - ptr += sizeof(double); - msg->twist.twist.linear.z = *((double *)ptr); - ptr += sizeof(double); - msg->twist.twist.angular.x = *((double *)ptr); - ptr += sizeof(double); - msg->twist.twist.angular.y = *((double *)ptr); - ptr += sizeof(double); - msg->twist.twist.angular.z = *((double *)ptr); - ptr += sizeof(double); - - for (size_t j = 0; j < 36; j++) - { - msg->twist.covariance[j] = *((double *)ptr); - ptr += sizeof(double); - } - - return ptr - udp_recv_buf_; -} - -int deserializeMultiBsplines(traj_utils::MultiBsplinesPtr &msg) -{ - char *ptr = recv_buf_; - - ptr += sizeof(MESSAGE_TYPE); - - msg->drone_id_from = *((int32_t *)ptr); - ptr += sizeof(int32_t); - msg->traj.resize(*((size_t *)ptr)); - ptr += sizeof(size_t); - for (size_t i = 0; i < msg->traj.size(); i++) - { - msg->traj[i].drone_id = *((int32_t *)ptr); - ptr += sizeof(int32_t); - msg->traj[i].order = *((int32_t *)ptr); - ptr += sizeof(int32_t); - msg->traj[i].start_time.fromSec(*((double *)ptr)); - ptr += sizeof(double); - msg->traj[i].traj_id = *((int64_t *)ptr); - ptr += sizeof(int64_t); - msg->traj[i].yaw_dt = *((double *)ptr); - ptr += sizeof(double); - - msg->traj[i].knots.resize(*((size_t *)ptr)); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->traj[i].knots.size(); j++) - { - msg->traj[i].knots[j] = *((double *)ptr); - ptr += sizeof(double); - } - - msg->traj[i].pos_pts.resize(*((size_t *)ptr)); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->traj[i].pos_pts.size(); j++) - { - msg->traj[i].pos_pts[j].x = *((double *)ptr); - ptr += sizeof(double); - msg->traj[i].pos_pts[j].y = *((double *)ptr); - ptr += sizeof(double); - msg->traj[i].pos_pts[j].z = *((double *)ptr); - ptr += sizeof(double); - } - - msg->traj[i].yaw_pts.resize(*((size_t *)ptr)); - ptr += sizeof(size_t); - for (size_t j = 0; j < msg->traj[i].yaw_pts.size(); j++) - { - msg->traj[i].yaw_pts[j] = *((double *)ptr); - ptr += sizeof(double); - } - } - - return ptr - recv_buf_; -} - -void multitraj_sub_tcp_cb(const traj_utils::MultiBsplinesPtr &msg) -{ - int len = serializeMultiBsplines(msg); - if (send(send_sock_, send_buf_, len, 0) <= 0) - { - ROS_ERROR("TCP SEND ERROR!!!"); - } -} - -void odom_sub_udp_cb(const nav_msgs::OdometryPtr &msg) -{ - - static ros::Time t_last; - ros::Time t_now = ros::Time::now(); - if ((t_now - t_last).toSec() * odom_broadcast_freq_ < 1.0) - { - return; - } - t_last = t_now; - - msg->child_frame_id = string("drone_") + std::to_string(drone_id_); - - int len = serializeOdom(msg); - - if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0) - { - ROS_ERROR("UDP SEND ERROR (1)!!!"); - } -} - -void emergency_stop_sub_udp_cb(const std_msgs::EmptyPtr &msg) -{ - - int len = serializeStop(msg); - - if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0) - { - ROS_ERROR("UDP SEND ERROR (2)!!!"); - } -} - -void one_traj_sub_udp_cb(const traj_utils::BsplinePtr &msg) -{ - - int len = serializeOneTraj(msg); - - if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0) - { - ROS_ERROR("UDP SEND ERROR (3)!!!"); - } -} - -void server_fun() -{ - int valread; - - // Connect - if (wait_connection_from_previous_drone(PORT, server_fd_, recv_sock_) < 0) - { - ROS_ERROR("[bridge_node]Socket recever creation error!"); - exit(EXIT_FAILURE); - } - - while (true) - { - valread = read(recv_sock_, recv_buf_, BUF_LEN); - - if ( valread <= 0 ) - { - ROS_ERROR("Received message length <= 0, maybe connection has lost"); - close(recv_sock_); - close(server_fd_); - return; - } - - if (valread == deserializeMultiBsplines(bsplines_msg_)) - { - swarm_trajs_pub_.publish(*bsplines_msg_); - } - else - { - ROS_ERROR("Received message length not matches the sent one!!!"); - continue; - } - } -} - -void udp_recv_fun() -{ - int valread; - struct sockaddr_in addr_client; - socklen_t addr_len; - - // Connect - if (udp_bind_to_port(UDP_PORT, udp_server_fd_) < 0) - { - ROS_ERROR("[bridge_node]Socket recever creation error!"); - exit(EXIT_FAILURE); - } - - while (true) - { - if ((valread = recvfrom(udp_server_fd_, udp_recv_buf_, BUF_LEN, 0, (struct sockaddr *)&addr_client, (socklen_t *)&addr_len)) < 0) - { - perror("recvfrom error:"); - exit(EXIT_FAILURE); - } - - char *ptr = udp_recv_buf_; - switch (*((MESSAGE_TYPE *)ptr)) - { - case MESSAGE_TYPE::STOP: - { - - if (valread == sizeof(std_msgs::Empty)) - { - if (valread == deserializeStop(stop_msg_)) - { - emergency_stop_pub_.publish(*stop_msg_); - } - else - { - ROS_ERROR("Received message length not matches the sent one (1)!!!"); - continue; - } - } - - break; - } - - case MESSAGE_TYPE::ODOM: - { - if (valread == deserializeOdom(odom_msg_)) - { - other_odoms_pub_.publish(*odom_msg_); - } - else - { - ROS_ERROR("Received message length not matches the sent one (2)!!!"); - continue; - } - - break; - } - - case MESSAGE_TYPE::ONE_TRAJ: - { - - if ( valread == deserializeOneTraj(bspline_msg_) ) - { - one_traj_pub_.publish(*bspline_msg_); - } - else - { - ROS_ERROR("Received message length not matches the sent one (3)!!!"); - continue; - } - - break; - } - - default: - - //ROS_ERROR("Unknown received message???"); - - break; - } - } -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "rosmsg_tcp_bridge"); - ros::NodeHandle nh("~"); - - nh.param("next_drone_ip", tcp_ip_, string("127.0.0.1")); - nh.param("broadcast_ip", udp_ip_, string("127.0.0.255")); - nh.param("drone_id", drone_id_, -1); - nh.param("odom_max_freq", odom_broadcast_freq_, 1000.0); - - bsplines_msg_.reset(new traj_utils::MultiBsplines); - odom_msg_.reset(new nav_msgs::Odometry); - stop_msg_.reset(new std_msgs::Empty); - bspline_msg_.reset(new traj_utils::Bspline); - - if (drone_id_ == -1) - { - ROS_ERROR("Wrong drone_id!"); - exit(EXIT_FAILURE); - } - - string sub_traj_topic_name = string("/uav") + std::to_string(drone_id_) + string("_planning/swarm_trajs"); - swarm_trajs_sub_ = nh.subscribe(sub_traj_topic_name.c_str(), 10, multitraj_sub_tcp_cb, ros::TransportHints().tcpNoDelay()); - - if ( drone_id_ >= 2 ) - { - string pub_traj_topic_name = string("/uav") + std::to_string(drone_id_ - 1) + string("_planning/swarm_trajs"); - swarm_trajs_pub_ = nh.advertise(pub_traj_topic_name.c_str(), 10); - } - - // other_odoms_sub_ = nh.subscribe("my_odom", 10, odom_sub_udp_cb, ros::TransportHints().tcpNoDelay()); - // other_odoms_pub_ = nh.advertise("/others_odom", 10); - - //emergency_stop_sub_ = nh.subscribe("emergency_stop_broadcast", 10, emergency_stop_sub_udp_cb, ros::TransportHints().tcpNoDelay()); - //emergency_stop_pub_ = nh.advertise("emergency_stop_recv", 10); - - one_traj_sub_ = nh.subscribe("/broadcast_bspline", 100, one_traj_sub_udp_cb, ros::TransportHints().tcpNoDelay()); - one_traj_pub_ = nh.advertise("/broadcast_bspline2", 100); - - boost::thread recv_thd(server_fun); - recv_thd.detach(); - ros::Duration(0.1).sleep(); - boost::thread udp_recv_thd(udp_recv_fun); - udp_recv_thd.detach(); - ros::Duration(0.1).sleep(); - - // TCP connect - send_sock_ = connect_to_next_drone(tcp_ip_.c_str(), PORT); - - // UDP connect - udp_send_fd_ = init_broadcast(udp_ip_.c_str(), UDP_PORT); - - cout << "[rosmsg_tcp_bridge] start running" << endl; - - ros::spin(); - - close(send_sock_); - close(recv_sock_); - close(server_fd_); - close(udp_server_fd_); - close(udp_send_fd_); - - return 0; -} diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/CMakeLists.txt b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/CMakeLists.txt deleted file mode 100644 index 5f1f457f..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(traj_utils) - -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++11") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") - -find_package(catkin REQUIRED COMPONENTS - #bspline_opt - #path_searching - roscpp - std_msgs - geometry_msgs - message_generation - #cv_bridge -) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) - -# Generate messages in the 'msg' folder -add_message_files( - FILES - Bspline.msg - DataDisp.msg - MultiBsplines.msg - ) - -# Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES traj_utils - CATKIN_DEPENDS message_runtime -# DEPENDS system_lib -) - -include_directories( - SYSTEM - include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -link_directories(${PCL_LIBRARY_DIRS}) - -add_library( traj_utils - src/planning_visualization.cpp - src/polynomial_traj.cpp - ) -target_link_libraries( traj_utils - ${catkin_LIBRARIES} - ) diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/plan_container.hpp b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/plan_container.hpp deleted file mode 100644 index 4c249df1..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/plan_container.hpp +++ /dev/null @@ -1,233 +0,0 @@ -#ifndef _PLAN_CONTAINER_H_ -#define _PLAN_CONTAINER_H_ - -#include -#include -#include - -#include -#include - -using std::vector; - -namespace ego_planner -{ - - class GlobalTrajData - { - private: - public: - PolynomialTraj global_traj_; - vector local_traj_; - - double global_duration_; - ros::Time global_start_time_; - double local_start_time_, local_end_time_; - double time_increase_; - double last_time_inc_; - double last_progress_time_; - - GlobalTrajData(/* args */) {} - - ~GlobalTrajData() {} - - bool localTrajReachTarget() { return fabs(local_end_time_ - global_duration_) < 0.1; } - - void setGlobalTraj(const PolynomialTraj &traj, const ros::Time &time) - { - global_traj_ = traj; - global_traj_.init(); - global_duration_ = global_traj_.getTimeSum(); - global_start_time_ = time; - - local_traj_.clear(); - local_start_time_ = -1; - local_end_time_ = -1; - time_increase_ = 0.0; - last_time_inc_ = 0.0; - last_progress_time_ = 0.0; - } - - void setLocalTraj(UniformBspline traj, double local_ts, double local_te, double time_inc) - { - local_traj_.resize(3); - local_traj_[0] = traj; - local_traj_[1] = local_traj_[0].getDerivative(); - local_traj_[2] = local_traj_[1].getDerivative(); - - local_start_time_ = local_ts; - local_end_time_ = local_te; - global_duration_ += time_inc; - time_increase_ += time_inc; - last_time_inc_ = time_inc; - } - - Eigen::Vector3d getPosition(double t) - { - if (t >= -1e-3 && t <= local_start_time_) - { - return global_traj_.evaluate(t - time_increase_ + last_time_inc_); - } - else if (t >= local_end_time_ && t <= global_duration_ + 1e-3) - { - return global_traj_.evaluate(t - time_increase_); - } - else - { - double tm, tmp; - local_traj_[0].getTimeSpan(tm, tmp); - return local_traj_[0].evaluateDeBoor(tm + t - local_start_time_); - } - } - - Eigen::Vector3d getVelocity(double t) - { - if (t >= -1e-3 && t <= local_start_time_) - { - return global_traj_.evaluateVel(t); - } - else if (t >= local_end_time_ && t <= global_duration_ + 1e-3) - { - return global_traj_.evaluateVel(t - time_increase_); - } - else - { - double tm, tmp; - local_traj_[0].getTimeSpan(tm, tmp); - return local_traj_[1].evaluateDeBoor(tm + t - local_start_time_); - } - } - - Eigen::Vector3d getAcceleration(double t) - { - if (t >= -1e-3 && t <= local_start_time_) - { - return global_traj_.evaluateAcc(t); - } - else if (t >= local_end_time_ && t <= global_duration_ + 1e-3) - { - return global_traj_.evaluateAcc(t - time_increase_); - } - else - { - double tm, tmp; - local_traj_[0].getTimeSpan(tm, tmp); - return local_traj_[2].evaluateDeBoor(tm + t - local_start_time_); - } - } - - // get Bspline paramterization data of a local trajectory within a sphere - // start_t: start time of the trajectory - // dist_pt: distance between the discretized points - void getTrajByRadius(const double &start_t, const double &des_radius, const double &dist_pt, - vector &point_set, vector &start_end_derivative, - double &dt, double &seg_duration) - { - double seg_length = 0.0; // length of the truncated segment - double seg_time = 0.0; // duration of the truncated segment - double radius = 0.0; // distance to the first point of the segment - - double delt = 0.2; - Eigen::Vector3d first_pt = getPosition(start_t); // first point of the segment - Eigen::Vector3d prev_pt = first_pt; // previous point - Eigen::Vector3d cur_pt; // current point - - // go forward until the traj exceed radius or global time - - while (radius < des_radius && seg_time < global_duration_ - start_t - 1e-3) - { - seg_time += delt; - seg_time = min(seg_time, global_duration_ - start_t); - - cur_pt = getPosition(start_t + seg_time); - seg_length += (cur_pt - prev_pt).norm(); - prev_pt = cur_pt; - radius = (cur_pt - first_pt).norm(); - } - - // get parameterization dt by desired density of points - int seg_num = floor(seg_length / dist_pt); - - // get outputs - - seg_duration = seg_time; // duration of the truncated segment - dt = seg_time / seg_num; // time difference between two points - - for (double tp = 0.0; tp <= seg_time + 1e-4; tp += dt) - { - cur_pt = getPosition(start_t + tp); - point_set.push_back(cur_pt); - } - - start_end_derivative.push_back(getVelocity(start_t)); - start_end_derivative.push_back(getVelocity(start_t + seg_time)); - start_end_derivative.push_back(getAcceleration(start_t)); - start_end_derivative.push_back(getAcceleration(start_t + seg_time)); - } - - // get Bspline paramterization data of a fixed duration local trajectory - // start_t: start time of the trajectory - // duration: time length of the segment - // seg_num: discretized the segment into *seg_num* parts - void getTrajByDuration(double start_t, double duration, int seg_num, - vector &point_set, - vector &start_end_derivative, double &dt) - { - dt = duration / seg_num; - Eigen::Vector3d cur_pt; - for (double tp = 0.0; tp <= duration + 1e-4; tp += dt) - { - cur_pt = getPosition(start_t + tp); - point_set.push_back(cur_pt); - } - - start_end_derivative.push_back(getVelocity(start_t)); - start_end_derivative.push_back(getVelocity(start_t + duration)); - start_end_derivative.push_back(getAcceleration(start_t)); - start_end_derivative.push_back(getAcceleration(start_t + duration)); - } - }; - - struct PlanParameters - { - /* planning algorithm parameters */ - double max_vel_, max_acc_, max_jerk_; // physical limits - double ctrl_pt_dist; // distance between adjacient B-spline control points - double feasibility_tolerance_; // permitted ratio of vel/acc exceeding limits - double planning_horizen_; - bool use_distinctive_trajs; - int drone_id; // single drone: drone_id <= -1, swarm: drone_id >= 0 - - /* processing time */ - double time_search_ = 0.0; - double time_optimize_ = 0.0; - double time_adjust_ = 0.0; - }; - - struct LocalTrajData - { - /* info of generated traj */ - - int traj_id_; - double duration_; - ros::Time start_time_; - Eigen::Vector3d start_pos_; - UniformBspline position_traj_, velocity_traj_, acceleration_traj_; - }; - - struct OneTrajDataOfSwarm - { - /* info of generated traj */ - - int drone_id; - double duration_; - ros::Time start_time_; - Eigen::Vector3d start_pos_; - UniformBspline position_traj_; - }; - - typedef std::vector SwarmTrajData; - -} // namespace ego_planner - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/planning_visualization.h b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/planning_visualization.h deleted file mode 100644 index 06d755b4..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/planning_visualization.h +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef _PLANNING_VISUALIZATION_H_ -#define _PLANNING_VISUALIZATION_H_ - -#include -#include -//#include -#include -//#include -#include -#include -#include -#include -#include - -using std::vector; -namespace ego_planner -{ - class PlanningVisualization - { - private: - ros::NodeHandle node; - - ros::Publisher goal_point_pub; - ros::Publisher global_list_pub; - ros::Publisher init_list_pub; - ros::Publisher optimal_list_pub; - ros::Publisher a_star_list_pub; - ros::Publisher guide_vector_pub; - ros::Publisher intermediate_state_pub; - - public: - PlanningVisualization(/* args */) {} - ~PlanningVisualization() {} - PlanningVisualization(ros::NodeHandle &nh); - - typedef std::shared_ptr Ptr; - - void displayMarkerList(ros::Publisher &pub, const vector &list, double scale, - Eigen::Vector4d color, int id, bool show_sphere = true); - void generatePathDisplayArray(visualization_msgs::MarkerArray &array, - const vector &list, double scale, Eigen::Vector4d color, int id); - void generateArrowDisplayArray(visualization_msgs::MarkerArray &array, - const vector &list, double scale, Eigen::Vector4d color, int id); - void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id); - void displayGlobalPathList(vector global_pts, const double scale, int id); - void displayInitPathList(vector init_pts, const double scale, int id); - void displayMultiInitPathList(vector> init_trajs, const double scale); - void displayOptimalList(Eigen::MatrixXd optimal_pts, int id); - void displayAStarList(std::vector> a_star_paths, int id); - void displayArrowList(ros::Publisher &pub, const vector &list, double scale, Eigen::Vector4d color, int id); - // void displayIntermediateState(ros::Publisher& intermediate_pub, ego_planner::BsplineOptimizer::Ptr optimizer, double sleep_time, const int start_iteration); - // void displayNewArrow(ros::Publisher& guide_vector_pub, ego_planner::BsplineOptimizer::Ptr optimizer); - }; -} // namespace ego_planner -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/polynomial_traj.h b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/polynomial_traj.h deleted file mode 100644 index 72bac9b4..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/include/traj_utils/polynomial_traj.h +++ /dev/null @@ -1,336 +0,0 @@ -#ifndef _POLYNOMIAL_TRAJ_H -#define _POLYNOMIAL_TRAJ_H - -#include -#include - -using std::vector; - -class PolynomialTraj -{ -private: - vector times; // time of each segment - vector> cxs; // coefficient of x of each segment, from n-1 -> 0 - vector> cys; // coefficient of y of each segment - vector> czs; // coefficient of z of each segment - - double time_sum; - int num_seg; - - /* evaluation */ - vector traj_vec3d; - double length; - -public: - PolynomialTraj(/* args */) - { - } - ~PolynomialTraj() - { - } - - void reset() - { - times.clear(), cxs.clear(), cys.clear(), czs.clear(); - time_sum = 0.0, num_seg = 0; - } - - void addSegment(vector cx, vector cy, vector cz, double t) - { - cxs.push_back(cx), cys.push_back(cy), czs.push_back(cz), times.push_back(t); - } - - void init() - { - num_seg = times.size(); - time_sum = 0.0; - for (int i = 0; i < times.size(); ++i) - { - time_sum += times[i]; - } - } - - vector getTimes() - { - return times; - } - - vector> getCoef(int axis) - { - switch (axis) - { - case 0: - return cxs; - case 1: - return cys; - case 2: - return czs; - default: - std::cout << "\033[31mIllegal axis!\033[0m" << std::endl; - } - - vector> empty; - return empty; - } - - Eigen::Vector3d evaluate(double t) - { - /* detetrmine segment num */ - int idx = 0; - while (times[idx] + 1e-4 < t) - { - t -= times[idx]; - ++idx; - } - - /* evaluation */ - int order = cxs[idx].size(); - Eigen::VectorXd cx(order), cy(order), cz(order), tv(order); - for (int i = 0; i < order; ++i) - { - cx(i) = cxs[idx][i], cy(i) = cys[idx][i], cz(i) = czs[idx][i]; - tv(order - 1 - i) = std::pow(t, double(i)); - } - - Eigen::Vector3d pt; - pt(0) = tv.dot(cx), pt(1) = tv.dot(cy), pt(2) = tv.dot(cz); - return pt; - } - - Eigen::Vector3d evaluateVel(double t) - { - /* detetrmine segment num */ - int idx = 0; - while (times[idx] + 1e-4 < t) - { - t -= times[idx]; - ++idx; - } - - /* evaluation */ - int order = cxs[idx].size(); - Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1); - - /* coef of vel */ - for (int i = 0; i < order - 1; ++i) - { - vx(i) = double(i + 1) * cxs[idx][order - 2 - i]; - vy(i) = double(i + 1) * cys[idx][order - 2 - i]; - vz(i) = double(i + 1) * czs[idx][order - 2 - i]; - } - double ts = t; - Eigen::VectorXd tv(order - 1); - for (int i = 0; i < order - 1; ++i) - tv(i) = pow(ts, i); - - Eigen::Vector3d vel; - vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz); - return vel; - } - - Eigen::Vector3d evaluateAcc(double t) - { - /* detetrmine segment num */ - int idx = 0; - while (times[idx] + 1e-4 < t) - { - t -= times[idx]; - ++idx; - } - - /* evaluation */ - int order = cxs[idx].size(); - Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2); - - /* coef of vel */ - for (int i = 0; i < order - 2; ++i) - { - ax(i) = double((i + 2) * (i + 1)) * cxs[idx][order - 3 - i]; - ay(i) = double((i + 2) * (i + 1)) * cys[idx][order - 3 - i]; - az(i) = double((i + 2) * (i + 1)) * czs[idx][order - 3 - i]; - } - double ts = t; - Eigen::VectorXd tv(order - 2); - for (int i = 0; i < order - 2; ++i) - tv(i) = pow(ts, i); - - Eigen::Vector3d acc; - acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az); - return acc; - } - - /* for evaluating traj, should be called in sequence!!! */ - double getTimeSum() - { - return this->time_sum; - } - - vector getTraj() - { - double eval_t = 0.0; - traj_vec3d.clear(); - while (eval_t < time_sum) - { - Eigen::Vector3d pt = evaluate(eval_t); - traj_vec3d.push_back(pt); - eval_t += 0.01; - } - return traj_vec3d; - } - - double getLength() - { - length = 0.0; - - Eigen::Vector3d p_l = traj_vec3d[0], p_n; - for (int i = 1; i < traj_vec3d.size(); ++i) - { - p_n = traj_vec3d[i]; - length += (p_n - p_l).norm(); - p_l = p_n; - } - return length; - } - - double getMeanVel() - { - double mean_vel = length / time_sum; - } - - double getAccCost() - { - double cost = 0.0; - int order = cxs[0].size(); - - for (int s = 0; s < times.size(); ++s) - { - Eigen::Vector3d um; - um(0) = 2 * cxs[s][order - 3], um(1) = 2 * cys[s][order - 3], um(2) = 2 * czs[s][order - 3]; - cost += um.squaredNorm() * times[s]; - } - - return cost; - } - - double getJerk() - { - double jerk = 0.0; - - /* evaluate jerk */ - for (int s = 0; s < times.size(); ++s) - { - Eigen::VectorXd cxv(cxs[s].size()), cyv(cys[s].size()), czv(czs[s].size()); - /* convert coefficient */ - int order = cxs[s].size(); - for (int j = 0; j < order; ++j) - { - cxv(j) = cxs[s][order - 1 - j], cyv(j) = cys[s][order - 1 - j], czv(j) = czs[s][order - 1 - j]; - } - double ts = times[s]; - - /* jerk matrix */ - Eigen::MatrixXd mat_jerk(order, order); - mat_jerk.setZero(); - for (double i = 3; i < order; i += 1) - for (double j = 3; j < order; j += 1) - { - mat_jerk(i, j) = - i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) * pow(ts, i + j - 5) / (i + j - 5); - } - - jerk += (cxv.transpose() * mat_jerk * cxv)(0, 0); - jerk += (cyv.transpose() * mat_jerk * cyv)(0, 0); - jerk += (czv.transpose() * mat_jerk * czv)(0, 0); - } - - return jerk; - } - - void getMeanAndMaxVel(double &mean_v, double &max_v) - { - int num = 0; - mean_v = 0.0, max_v = -1.0; - for (int s = 0; s < times.size(); ++s) - { - int order = cxs[s].size(); - Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1); - - /* coef of vel */ - for (int i = 0; i < order - 1; ++i) - { - vx(i) = double(i + 1) * cxs[s][order - 2 - i]; - vy(i) = double(i + 1) * cys[s][order - 2 - i]; - vz(i) = double(i + 1) * czs[s][order - 2 - i]; - } - double ts = times[s]; - - double eval_t = 0.0; - while (eval_t < ts) - { - Eigen::VectorXd tv(order - 1); - for (int i = 0; i < order - 1; ++i) - tv(i) = pow(ts, i); - Eigen::Vector3d vel; - vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz); - double vn = vel.norm(); - mean_v += vn; - if (vn > max_v) - max_v = vn; - ++num; - - eval_t += 0.01; - } - } - - mean_v = mean_v / double(num); - } - - void getMeanAndMaxAcc(double &mean_a, double &max_a) - { - int num = 0; - mean_a = 0.0, max_a = -1.0; - for (int s = 0; s < times.size(); ++s) - { - int order = cxs[s].size(); - Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2); - - /* coef of acc */ - for (int i = 0; i < order - 2; ++i) - { - ax(i) = double((i + 2) * (i + 1)) * cxs[s][order - 3 - i]; - ay(i) = double((i + 2) * (i + 1)) * cys[s][order - 3 - i]; - az(i) = double((i + 2) * (i + 1)) * czs[s][order - 3 - i]; - } - double ts = times[s]; - - double eval_t = 0.0; - while (eval_t < ts) - { - Eigen::VectorXd tv(order - 2); - for (int i = 0; i < order - 2; ++i) - tv(i) = pow(ts, i); - Eigen::Vector3d acc; - acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az); - double an = acc.norm(); - mean_a += an; - if (an > max_a) - max_a = an; - ++num; - - eval_t += 0.01; - } - } - - mean_a = mean_a / double(num); - } - - static PolynomialTraj minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel, - const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time); - - static PolynomialTraj one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc, - double t); -}; - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/Bspline.msg b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/Bspline.msg deleted file mode 100644 index df4cfd40..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/Bspline.msg +++ /dev/null @@ -1,12 +0,0 @@ -int32 drone_id - -int32 order -int64 traj_id -time start_time - -float64[] knots -geometry_msgs/Point[] pos_pts - -float64[] yaw_pts -float64 yaw_dt - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/DataDisp.msg b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/DataDisp.msg deleted file mode 100644 index 742f28ad..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/DataDisp.msg +++ /dev/null @@ -1,7 +0,0 @@ -Header header - -float64 a -float64 b -float64 c -float64 d -float64 e \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/MultiBsplines.msg b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/MultiBsplines.msg deleted file mode 100644 index 2f116f21..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/msg/MultiBsplines.msg +++ /dev/null @@ -1,4 +0,0 @@ -int32 drone_id_from - -Bspline[] traj - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/package.xml b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/package.xml deleted file mode 100644 index 481ac269..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/package.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - traj_utils - 0.0.0 - The traj_utils package - - - - - bzhouai - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - roscpp - std_msgs - message_generation - - catkin - - - - std_msgs - - catkin - - - roscpp - std_msgs - message_runtime - - - - - - - - diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/src/planning_visualization.cpp b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/src/planning_visualization.cpp deleted file mode 100644 index 888320cb..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/src/planning_visualization.cpp +++ /dev/null @@ -1,265 +0,0 @@ -#include - -using std::cout; -using std::endl; -namespace ego_planner -{ - PlanningVisualization::PlanningVisualization(ros::NodeHandle &nh) - { - node = nh; - - goal_point_pub = nh.advertise("goal_point", 2); - global_list_pub = nh.advertise("global_list", 2); - init_list_pub = nh.advertise("init_list", 2); - optimal_list_pub = nh.advertise("optimal_list", 2); - a_star_list_pub = nh.advertise("a_star_list", 20); - } - - // // real ids used: {id, id+1000} - void PlanningVisualization::displayMarkerList(ros::Publisher &pub, const vector &list, double scale, - Eigen::Vector4d color, int id, bool show_sphere /* = true */ ) - { - visualization_msgs::Marker sphere, line_strip; - sphere.header.frame_id = line_strip.header.frame_id = "world"; - sphere.header.stamp = line_strip.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE_LIST; - line_strip.type = visualization_msgs::Marker::LINE_STRIP; - sphere.action = line_strip.action = visualization_msgs::Marker::ADD; - sphere.id = id; - line_strip.id = id + 1000; - - sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0; - sphere.color.r = line_strip.color.r = color(0); - sphere.color.g = line_strip.color.g = color(1); - sphere.color.b = line_strip.color.b = color(2); - sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0; - sphere.scale.x = scale; - sphere.scale.y = scale; - sphere.scale.z = scale; - line_strip.scale.x = scale / 2; - geometry_msgs::Point pt; - for (int i = 0; i < int(list.size()); i++) - { - pt.x = list[i](0); - pt.y = list[i](1); - pt.z = list[i](2); - //if (show_sphere) sphere.points.push_back(pt); - line_strip.points.push_back(pt); - } - //if (show_sphere) pub.publish(sphere); - pub.publish(line_strip); - } - - // real ids used: {id, id+1} - void PlanningVisualization::generatePathDisplayArray(visualization_msgs::MarkerArray &array, - const vector &list, double scale, Eigen::Vector4d color, int id) - { - visualization_msgs::Marker sphere, line_strip; - sphere.header.frame_id = line_strip.header.frame_id = "map"; - sphere.header.stamp = line_strip.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE_LIST; - line_strip.type = visualization_msgs::Marker::LINE_STRIP; - sphere.action = line_strip.action = visualization_msgs::Marker::ADD; - sphere.id = id; - line_strip.id = id + 1; - - sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0; - sphere.color.r = line_strip.color.r = color(0); - sphere.color.g = line_strip.color.g = color(1); - sphere.color.b = line_strip.color.b = color(2); - sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0; - sphere.scale.x = scale; - sphere.scale.y = scale; - sphere.scale.z = scale; - line_strip.scale.x = scale / 3; - geometry_msgs::Point pt; - for (int i = 0; i < int(list.size()); i++) - { - pt.x = list[i](0); - pt.y = list[i](1); - pt.z = list[i](2); - sphere.points.push_back(pt); - line_strip.points.push_back(pt); - } - array.markers.push_back(sphere); - array.markers.push_back(line_strip); - } - - // real ids used: {1000*id ~ (arrow nums)+1000*id} - void PlanningVisualization::generateArrowDisplayArray(visualization_msgs::MarkerArray &array, - const vector &list, double scale, Eigen::Vector4d color, int id) - { - visualization_msgs::Marker arrow; - arrow.header.frame_id = "map"; - arrow.header.stamp = ros::Time::now(); - arrow.type = visualization_msgs::Marker::ARROW; - arrow.action = visualization_msgs::Marker::ADD; - - // geometry_msgs::Point start, end; - // arrow.points - - arrow.color.r = color(0); - arrow.color.g = color(1); - arrow.color.b = color(2); - arrow.color.a = color(3) > 1e-5 ? color(3) : 1.0; - arrow.scale.x = scale; - arrow.scale.y = 2 * scale; - arrow.scale.z = 2 * scale; - - geometry_msgs::Point start, end; - for (int i = 0; i < int(list.size() / 2); i++) - { - // arrow.color.r = color(0) / (1+i); - // arrow.color.g = color(1) / (1+i); - // arrow.color.b = color(2) / (1+i); - - start.x = list[2 * i](0); - start.y = list[2 * i](1); - start.z = list[2 * i](2); - end.x = list[2 * i + 1](0); - end.y = list[2 * i + 1](1); - end.z = list[2 * i + 1](2); - arrow.points.clear(); - arrow.points.push_back(start); - arrow.points.push_back(end); - arrow.id = i + id * 1000; - - array.markers.push_back(arrow); - } - } - - void PlanningVisualization::displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id) - { - visualization_msgs::Marker sphere; - sphere.header.frame_id = "world"; - sphere.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE; - sphere.action = visualization_msgs::Marker::ADD; - sphere.id = id; - - sphere.pose.orientation.w = 1.0; - sphere.color.r = color(0); - sphere.color.g = color(1); - sphere.color.b = color(2); - sphere.color.a = color(3); - sphere.scale.x = scale; - sphere.scale.y = scale; - sphere.scale.z = scale; - sphere.pose.position.x = goal_point(0); - sphere.pose.position.y = goal_point(1); - sphere.pose.position.z = goal_point(2); - - goal_point_pub.publish(sphere); - } - - void PlanningVisualization::displayGlobalPathList(vector init_pts, const double scale, int id) - { - - if (global_list_pub.getNumSubscribers() == 0) - { - return; - } - - Eigen::Vector4d color(0, 0.5, 0.5, 1); - displayMarkerList(global_list_pub, init_pts, scale, color, id); - } - - void PlanningVisualization::displayMultiInitPathList(vector> init_trajs, const double scale) - { - - if (init_list_pub.getNumSubscribers() == 0) - { - return; - } - - static int last_nums = 0; - - for ( int id=0; id blank; - displayMarkerList(init_list_pub, blank, scale, color, id, false); - ros::Duration(0.001).sleep(); - } - last_nums = 0; - - for ( int id=0; id init_pts, const double scale, int id) - { - - if (init_list_pub.getNumSubscribers() == 0) - { - return; - } - - Eigen::Vector4d color(0, 0, 1, 1); - displayMarkerList(init_list_pub, init_pts, scale, color, id); - } - - void PlanningVisualization::displayOptimalList(Eigen::MatrixXd optimal_pts, int id) - { - - if (optimal_list_pub.getNumSubscribers() == 0) - { - return; - } - - vector list; - for (int i = 0; i < optimal_pts.cols(); i++) - { - Eigen::Vector3d pt = optimal_pts.col(i).transpose(); - list.push_back(pt); - } - Eigen::Vector4d color(1, 0, 0, 1); - displayMarkerList(optimal_list_pub, list, 0.15, color, id); - } - - void PlanningVisualization::displayAStarList(std::vector> a_star_paths, int id /* = Eigen::Vector4d(0.5,0.5,0,1)*/) - { - - if (a_star_list_pub.getNumSubscribers() == 0) - { - return; - } - - int i = 0; - vector list; - - Eigen::Vector4d color = Eigen::Vector4d(0.5 + ((double)rand() / RAND_MAX / 2), 0.5 + ((double)rand() / RAND_MAX / 2), 0, 1); // make the A star pathes different every time. - double scale = 0.05 + (double)rand() / RAND_MAX / 10; - - for (auto block : a_star_paths) - { - list.clear(); - for (auto pt : block) - { - list.push_back(pt); - } - //Eigen::Vector4d color(0.5,0.5,0,1); - displayMarkerList(a_star_list_pub, list, scale, color, id + i); // real ids used: [ id ~ id+a_star_paths.size() ] - i++; - } - } - - void PlanningVisualization::displayArrowList(ros::Publisher &pub, const vector &list, double scale, Eigen::Vector4d color, int id) - { - visualization_msgs::MarkerArray array; - // clear - pub.publish(array); - - generateArrowDisplayArray(array, list, scale, color, id); - - pub.publish(array); - } - - // PlanningVisualization:: -} // namespace ego_planner \ No newline at end of file diff --git a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/src/polynomial_traj.cpp b/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/src/polynomial_traj.cpp deleted file mode 100644 index a83b7ae1..00000000 --- a/src/无人机端代码/Modules/ego_planner_swarm/traj_utils/src/polynomial_traj.cpp +++ /dev/null @@ -1,224 +0,0 @@ -#include -#include - -PolynomialTraj PolynomialTraj::minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel, - const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time) -{ - int seg_num = Time.size(); - Eigen::MatrixXd poly_coeff(seg_num, 3 * 6); - Eigen::VectorXd Px(6 * seg_num), Py(6 * seg_num), Pz(6 * seg_num); - - int num_f, num_p; // number of fixed and free variables - int num_d; // number of all segments' derivatives - - const static auto Factorial = [](int x) { - int fac = 1; - for (int i = x; i > 0; i--) - fac = fac * i; - return fac; - }; - - /* ---------- end point derivative ---------- */ - Eigen::VectorXd Dx = Eigen::VectorXd::Zero(seg_num * 6); - Eigen::VectorXd Dy = Eigen::VectorXd::Zero(seg_num * 6); - Eigen::VectorXd Dz = Eigen::VectorXd::Zero(seg_num * 6); - - for (int k = 0; k < seg_num; k++) - { - /* position to derivative */ - Dx(k * 6) = Pos(0, k); - Dx(k * 6 + 1) = Pos(0, k + 1); - Dy(k * 6) = Pos(1, k); - Dy(k * 6 + 1) = Pos(1, k + 1); - Dz(k * 6) = Pos(2, k); - Dz(k * 6 + 1) = Pos(2, k + 1); - - if (k == 0) - { - Dx(k * 6 + 2) = start_vel(0); - Dy(k * 6 + 2) = start_vel(1); - Dz(k * 6 + 2) = start_vel(2); - - Dx(k * 6 + 4) = start_acc(0); - Dy(k * 6 + 4) = start_acc(1); - Dz(k * 6 + 4) = start_acc(2); - } - else if (k == seg_num - 1) - { - Dx(k * 6 + 3) = end_vel(0); - Dy(k * 6 + 3) = end_vel(1); - Dz(k * 6 + 3) = end_vel(2); - - Dx(k * 6 + 5) = end_acc(0); - Dy(k * 6 + 5) = end_acc(1); - Dz(k * 6 + 5) = end_acc(2); - } - } - - /* ---------- Mapping Matrix A ---------- */ - Eigen::MatrixXd Ab; - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6); - - for (int k = 0; k < seg_num; k++) - { - Ab = Eigen::MatrixXd::Zero(6, 6); - for (int i = 0; i < 3; i++) - { - Ab(2 * i, i) = Factorial(i); - for (int j = i; j < 6; j++) - Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i); - } - A.block(k * 6, k * 6, 6, 6) = Ab; - } - - /* ---------- Produce Selection Matrix C' ---------- */ - Eigen::MatrixXd Ct, C; - - num_f = 2 * seg_num + 4; // 3 + 3 + (seg_num - 1) * 2 = 2m + 4 - num_p = 2 * seg_num - 2; //(seg_num - 1) * 2 = 2m - 2 - num_d = 6 * seg_num; - Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p); - Ct(0, 0) = 1; - Ct(2, 1) = 1; - Ct(4, 2) = 1; // stack the start point - Ct(1, 3) = 1; - Ct(3, 2 * seg_num + 4) = 1; - Ct(5, 2 * seg_num + 5) = 1; - - Ct(6 * (seg_num - 1) + 0, 2 * seg_num + 0) = 1; - Ct(6 * (seg_num - 1) + 1, 2 * seg_num + 1) = 1; // Stack the end point - Ct(6 * (seg_num - 1) + 2, 4 * seg_num + 0) = 1; - Ct(6 * (seg_num - 1) + 3, 2 * seg_num + 2) = 1; // Stack the end point - Ct(6 * (seg_num - 1) + 4, 4 * seg_num + 1) = 1; - Ct(6 * (seg_num - 1) + 5, 2 * seg_num + 3) = 1; // Stack the end point - - for (int j = 2; j < seg_num; j++) - { - Ct(6 * (j - 1) + 0, 2 + 2 * (j - 1) + 0) = 1; - Ct(6 * (j - 1) + 1, 2 + 2 * (j - 1) + 1) = 1; - Ct(6 * (j - 1) + 2, 2 * seg_num + 4 + 2 * (j - 2) + 0) = 1; - Ct(6 * (j - 1) + 3, 2 * seg_num + 4 + 2 * (j - 1) + 0) = 1; - Ct(6 * (j - 1) + 4, 2 * seg_num + 4 + 2 * (j - 2) + 1) = 1; - Ct(6 * (j - 1) + 5, 2 * seg_num + 4 + 2 * (j - 1) + 1) = 1; - } - - C = Ct.transpose(); - - Eigen::VectorXd Dx1 = C * Dx; - Eigen::VectorXd Dy1 = C * Dy; - Eigen::VectorXd Dz1 = C * Dz; - - /* ---------- minimum snap matrix ---------- */ - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6); - - for (int k = 0; k < seg_num; k++) - { - for (int i = 3; i < 6; i++) - { - for (int j = 3; j < 6; j++) - { - Q(k * 6 + i, k * 6 + j) = - i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5)); - } - } - } - - /* ---------- R matrix ---------- */ - Eigen::MatrixXd R = C * A.transpose().inverse() * Q * A.inverse() * Ct; - - Eigen::VectorXd Dxf(2 * seg_num + 4), Dyf(2 * seg_num + 4), Dzf(2 * seg_num + 4); - - Dxf = Dx1.segment(0, 2 * seg_num + 4); - Dyf = Dy1.segment(0, 2 * seg_num + 4); - Dzf = Dz1.segment(0, 2 * seg_num + 4); - - Eigen::MatrixXd Rff(2 * seg_num + 4, 2 * seg_num + 4); - Eigen::MatrixXd Rfp(2 * seg_num + 4, 2 * seg_num - 2); - Eigen::MatrixXd Rpf(2 * seg_num - 2, 2 * seg_num + 4); - Eigen::MatrixXd Rpp(2 * seg_num - 2, 2 * seg_num - 2); - - Rff = R.block(0, 0, 2 * seg_num + 4, 2 * seg_num + 4); - Rfp = R.block(0, 2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2); - Rpf = R.block(2 * seg_num + 4, 0, 2 * seg_num - 2, 2 * seg_num + 4); - Rpp = R.block(2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2, 2 * seg_num - 2); - - /* ---------- close form solution ---------- */ - - Eigen::VectorXd Dxp(2 * seg_num - 2), Dyp(2 * seg_num - 2), Dzp(2 * seg_num - 2); - Dxp = -(Rpp.inverse() * Rfp.transpose()) * Dxf; - Dyp = -(Rpp.inverse() * Rfp.transpose()) * Dyf; - Dzp = -(Rpp.inverse() * Rfp.transpose()) * Dzf; - - Dx1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dxp; - Dy1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dyp; - Dz1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dzp; - - Px = (A.inverse() * Ct) * Dx1; - Py = (A.inverse() * Ct) * Dy1; - Pz = (A.inverse() * Ct) * Dz1; - - for (int i = 0; i < seg_num; i++) - { - poly_coeff.block(i, 0, 1, 6) = Px.segment(i * 6, 6).transpose(); - poly_coeff.block(i, 6, 1, 6) = Py.segment(i * 6, 6).transpose(); - poly_coeff.block(i, 12, 1, 6) = Pz.segment(i * 6, 6).transpose(); - } - - /* ---------- use polynomials ---------- */ - PolynomialTraj poly_traj; - for (int i = 0; i < poly_coeff.rows(); ++i) - { - vector cx(6), cy(6), cz(6); - for (int j = 0; j < 6; ++j) - { - cx[j] = poly_coeff(i, j), cy[j] = poly_coeff(i, j + 6), cz[j] = poly_coeff(i, j + 12); - } - reverse(cx.begin(), cx.end()); - reverse(cy.begin(), cy.end()); - reverse(cz.begin(), cz.end()); - double ts = Time(i); - poly_traj.addSegment(cx, cy, cz, ts); - } - - return poly_traj; -} - -PolynomialTraj PolynomialTraj::one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc, - double t) -{ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(6, 6), Crow(1, 6); - Eigen::VectorXd Bx(6), By(6), Bz(6); - - C(0, 5) = 1; - C(1, 4) = 1; - C(2, 3) = 2; - Crow << pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1; - C.row(3) = Crow; - Crow << 5 * pow(t, 4), 4 * pow(t, 3), 3 * pow(t, 2), 2 * t, 1, 0; - C.row(4) = Crow; - Crow << 20 * pow(t, 3), 12 * pow(t, 2), 6 * t, 2, 0, 0; - C.row(5) = Crow; - - Bx << start_pt(0), start_vel(0), start_acc(0), end_pt(0), end_vel(0), end_acc(0); - By << start_pt(1), start_vel(1), start_acc(1), end_pt(1), end_vel(1), end_acc(1); - Bz << start_pt(2), start_vel(2), start_acc(2), end_pt(2), end_vel(2), end_acc(2); - - Eigen::VectorXd Cofx = C.colPivHouseholderQr().solve(Bx); - Eigen::VectorXd Cofy = C.colPivHouseholderQr().solve(By); - Eigen::VectorXd Cofz = C.colPivHouseholderQr().solve(Bz); - - vector cx(6), cy(6), cz(6); - for (int i = 0; i < 6; i++) - { - cx[i] = Cofx(i); - cy[i] = Cofy(i); - cz[i] = Cofz(i); - } - - PolynomialTraj poly_traj; - poly_traj.addSegment(cx, cy, cz, t); - - return poly_traj; -} diff --git a/src/无人机端代码/Modules/tutorial_demo/CMakeLists.txt b/src/无人机端代码/Modules/tutorial_demo/CMakeLists.txt deleted file mode 100644 index cc64a801..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/CMakeLists.txt +++ /dev/null @@ -1,162 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(prometheus_demo) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - message_generation - roscpp - rospy - geometry_msgs - sensor_msgs - mavros - nav_msgs - std_msgs - std_srvs - tf2_ros - tf2_eigen - mavros_msgs - prometheus_msgs -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - geometry_msgs - sensor_msgs - std_msgs -) - -catkin_package( - CATKIN_DEPENDS message_runtime -) - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - advanced/autonomous_landing/include - ${catkin_INCLUDE_DIRS} - ../common/include -) - - -############################### -## 声明可执行cpp文件 ## -############################### -# add_executable(autonomous_landing autonomous_landing/autonomous_landing.cpp) -# target_link_libraries(autonomous_landing ${catkin_LIBRARIES}) - -#add_executable(waypoint_tracking waypoint_tracking/waypoint_tracking.cpp) -#target_link_libraries(waypoint_tracking ${catkin_LIBRARIES}) - -#add_executable(object_tracking object_tracking/object_tracking.cpp) -#target_link_libraries(object_tracking ${catkin_LIBRARIES}) - -#add_executable(takeoff takeoff/takeoff.cpp) -#target_link_libraries(takeoff ${catkin_LIBRARIES}) - -#add_executable(formation_control formation_control/formation_control.cpp) -#target_link_libraries(formation_control ${catkin_LIBRARIES}) - -add_executable(takeoff_land basic/takeoff_land/src/takeoff_land.cpp) -target_link_libraries(takeoff_land ${catkin_LIBRARIES}) - -add_executable(takeoff_land_liu basic/takeoff_land/src/takeoff_land_liu.cpp) -target_link_libraries(takeoff_land_liu ${catkin_LIBRARIES}) - -add_executable(takeoff_land_no_rc basic/takeoff_land/src/takeoff_land_no_rc.cpp) -target_link_libraries(takeoff_land_no_rc ${catkin_LIBRARIES}) - -add_executable(global_pos_control basic/global_pos_control/src/global_pos_control.cpp) -target_link_libraries(global_pos_control ${catkin_LIBRARIES}) - -add_executable(enu_xyz_pos_control basic/enu_xyz_pos_control/src/enu_xyz_pos_control.cpp) -target_link_libraries(enu_xyz_pos_control ${catkin_LIBRARIES}) - -add_executable(body_xyz_pos_control basic/body_xyz_pos_control/src/body_xyz_pos_control.cpp) -target_link_libraries(body_xyz_pos_control ${catkin_LIBRARIES}) - -add_executable(circular_trajectory_control basic/circular_trajectory_control/src/circular_trajectory_control.cpp) -target_link_libraries(circular_trajectory_control ${catkin_LIBRARIES}) - -add_executable(formation_control advanced/formation_control/src/formation_control.cpp) -target_link_libraries(formation_control ${catkin_LIBRARIES}) - -add_executable(autonomous_landing advanced/autonomous_landing/src/autonomous_landing.cpp) -target_link_libraries(autonomous_landing ${catkin_LIBRARIES}) - -add_executable(siamrpn_track advanced/siamrpn_track/src/siamrpn_track.cpp) -target_link_libraries(siamrpn_track ${catkin_LIBRARIES}) - -add_executable(find_aruco_marker advanced/find_aruco_marker/src/find_aruco_marker.cpp) -target_link_libraries(find_aruco_marker ${catkin_LIBRARIES}) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS - # advanced/future_aircraft/scripts/locus.py - # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_prometheus_demo.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/include/mission_utils.h b/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/include/mission_utils.h deleted file mode 100644 index 7c764a5a..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/include/mission_utils.h +++ /dev/null @@ -1,184 +0,0 @@ -/*************************************************************************************************************************** -* mission_utils.h -* -* Author: Qyp -* -* Update Time: 2020.1.12 -* -* Introduction: mission_utils -* -***************************************************************************************************************************/ - -#ifndef MISSION_UTILS_H -#define MISSION_UTILS_H - -#include -#include - -//topic 头文件 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; - -#define DIS_THRES 0.1 -#define VISION_THRES 10 - -//相机安装OFFSET -#define FRONT_CAMERA_OFFSET_X 0.2 -#define FRONT_CAMERA_OFFSET_Y 0.0 -#define FRONT_CAMERA_OFFSET_Z -0.05 - -#define DOWN_CAMERA_OFFSET_X 0.0 -#define DOWN_CAMERA_OFFSET_Y 0.0 -#define DOWN_CAMERA_OFFSET_Z -0.05 - - -// 定义视觉检测结构体 -// -struct Detection_result -{ - string object_name; - // 视觉检测原始信息，返回的结果为相机坐标系 - // 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正） - // 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标 - prometheus_msgs::DetectionInfo Detection_info; - // 目标在相机系位置 - Eigen::Vector3f pos_camera_frame; - // 目标在机体系位置 - Eigen::Vector3f pos_body_frame; - // 目标在机体-惯性系位置 (原点位于质心，x轴指向前方，y轴指向左，z轴指向上的坐标系) - Eigen::Vector3f pos_body_enu_frame; - // 目标在惯性系位置 (原点位于起飞点，x轴指向前方，y轴指向左，z轴指向上的坐标系) - Eigen::Vector3f pos_enu_frame; - // 目标在机体系姿态 - Eigen::Vector3f att_body_frame; - // 目标在惯性系姿态 - Eigen::Vector3f att_enu_frame; - // 目标识别标志位,阈值:VISION_THRES - bool is_detected = false; - int num_lost = 0; //视觉丢失计数器 - int num_regain = 0; -}; - - -//打印视觉检测消息 -void printf_detection_result(const struct Detection_result& det_info) -{ - cout << "Object_name : " << det_info.object_name << endl; - if(det_info.is_detected) - { - cout << "is_detected : " << "True" << endl; - }else - { - cout << "is_detected : " << "False" << endl; - } - cout << "Camera_frame : " << det_info.Detection_info.position[0] << " [m] "<< det_info.Detection_info.position[1] << " [m] "<< det_info.Detection_info.position[2] << " [m] "<Max) - { - return (data > 0) ? Max : -Max; - } - else - { - return data; - } -} - -//constrain_function2 -float constrain_function2(float data, float Min,float Max) -{ - if(data > Max) - { - return Max; - } - else if (data < Min) - { - return Min; - }else - { - return data; - } -} - -//sign_function -float sign_function(float data) -{ - if(data>0) - { - return 1.0; - } - else if(data<0) - { - return -1.0; - } - else if(data == 0) - { - return 0.0; - } -} - -// min function -float min(float data1,float data2) -{ - if(data1>=data2) - { - return data2; - } - else - { - return data1; - } -} - -//旋转矩阵：机体系到惯性系 -Eigen::Matrix3f get_rotation_matrix(float phi, float theta, float psi) -{ - Eigen::Matrix3f R_Body_to_ENU; - - float r11 = cos(theta)*cos(psi); - float r12 = - cos(phi)*sin(psi) + sin(phi)*sin(theta)*cos(psi); - float r13 = sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi); - float r21 = cos(theta)*sin(psi); - float r22 = cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi); - float r23 = - sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi); - float r31 = - sin(theta); - float r32 = sin(phi)*cos(theta); - float r33 = cos(phi)*cos(theta); - R_Body_to_ENU << r11,r12,r13,r21,r22,r23,r31,r32,r33; - - return R_Body_to_ENU; -} -#endif diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/launch/autonomous_landing_all.launch b/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/launch/autonomous_landing_all.launch deleted file mode 100644 index 581d3327..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/launch/autonomous_landing_all.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/src/autonomous_landing.cpp b/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/src/autonomous_landing.cpp deleted file mode 100644 index 652d304e..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/autonomous_landing/src/autonomous_landing.cpp +++ /dev/null @@ -1,313 +0,0 @@ -#include -#include -#include -#include "prometheus_msgs/UAVState.h" -#include "prometheus_msgs/UAVCommand.h" -#include "prometheus_msgs/UAVControlState.h" -#include "printf_utils.h" - -#include "mission_utils.h" - -using namespace std; -using namespace Eigen; -#define NODE_NAME "autonomous_landing" - -//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< -bool g_use_pad_height; // 是否使用降落板绝对高度 -float g_pad_height; -int g_uav_id; -std_msgs::Bool vision_switch; -float max_height; // 起始降落位置 -float g_camera_offset[3]; -prometheus_msgs::UAVCommand g_command_now; -prometheus_msgs::UAVControlState g_uavcontrol_state; //目标位置[机体系下：前方x为正，右方y为正，下方z为正] -//---------------------------------------Drone--------------------------------------------- -prometheus_msgs::UAVState g_UAVState; // 无人机状态 -Eigen::Matrix3f g_R_Body_to_ENU; // 无人机机体系至惯性系转换矩阵 -//---------------------------------------Vision--------------------------------------------- -nav_msgs::Odometry g_GroundTruth; // 降落板真实位置（仿真中由Gazebo插件提供） -Detection_result g_landpad_det; // 检测结果 -//---------------------------------------Track--------------------------------------------- -float g_kp_land[3]; // 控制参数，控制无人机的速度 - -// 四种状态机 -enum EXEC_STATE -{ - WAITING, - TRACKING, - LOST, - LANDING, -}; -EXEC_STATE exec_state; - -float g_distance_to_pad; -float g_arm_height_to_ground; -float g_arm_distance_to_pad; -//---------------------------------------Output--------------------------------------------- -//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< -void landpadDetCb(const prometheus_msgs::DetectionInfo::ConstPtr &msg) -{ - g_landpad_det.object_name = "landpad"; - g_landpad_det.Detection_info = *msg; - // 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正） - // 相机安装误差 在mission_utils.h中设置场 - // x, y轴交换 - g_landpad_det.pos_body_frame[0] = -g_landpad_det.Detection_info.position[1] + g_camera_offset[0]; - g_landpad_det.pos_body_frame[1] = -g_landpad_det.Detection_info.position[0] + g_camera_offset[1]; - g_landpad_det.pos_body_frame[2] = -g_landpad_det.Detection_info.position[2] + g_camera_offset[2]; - - // 机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦) - g_landpad_det.pos_body_enu_frame = g_R_Body_to_ENU * g_landpad_det.pos_body_frame; - - if (g_use_pad_height) - { - //若已知降落板高度，则无需使用深度信息。 - g_landpad_det.pos_body_enu_frame[2] = g_pad_height - g_UAVState.position[2]; - } - - // 机体惯性系 -> 惯性系 - g_landpad_det.pos_enu_frame[0] = g_UAVState.position[0] + g_landpad_det.pos_body_enu_frame[0]; - g_landpad_det.pos_enu_frame[1] = g_UAVState.position[1] + g_landpad_det.pos_body_enu_frame[1]; - g_landpad_det.pos_enu_frame[2] = g_UAVState.position[2] + g_landpad_det.pos_body_enu_frame[2]; - // 此降落方案不考虑偏航角 - g_landpad_det.att_enu_frame[2] = 0.0; - - if (g_landpad_det.Detection_info.detected) - { - g_landpad_det.num_regain++; - g_landpad_det.num_lost = 0; - } - else - { - g_landpad_det.num_regain = 0; - g_landpad_det.num_lost++; - } - - // 当连续一段时间无法检测到目标时，认定目标丢失 - if (g_landpad_det.num_lost > VISION_THRES) - { - g_landpad_det.is_detected = false; - } - - // 当连续一段时间检测到目标时，认定目标得到 - if (g_landpad_det.num_regain > VISION_THRES) - { - g_landpad_det.is_detected = true; - } -} - -void droneStateCb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - g_UAVState = *msg; - - g_R_Body_to_ENU = get_rotation_matrix(g_UAVState.attitude[0], g_UAVState.attitude[1], g_UAVState.attitude[2]); -} - -inline void readParams(const ros::NodeHandle &nh) -{ - nh.param("uav_id", g_uav_id, 1); - //强制上锁高度 - nh.param("arm_height_to_ground", g_arm_height_to_ground, 0.4); - //强制上锁距离 - nh.param("arm_distance_to_pad", g_arm_distance_to_pad, 0.3); - // 是否使用降落板绝对高度 - nh.param("use_pad_height", g_use_pad_height, false); - nh.param("pad_height", g_pad_height, 0.01); - - //追踪控制参数 - nh.param("kpx_land", g_kp_land[0], 0.1); - nh.param("kpy_land", g_kp_land[1], 0.1); - nh.param("kpz_land", g_kp_land[2], 0.1); - - // 目标丢失时，最大到飞行高度，如果高于这个值判定为任务失败 - nh.param("max_height", max_height, 3.0); - - // 相机安装偏移,规定为:相机在机体系(质心原点)的位置 - nh.param("camera_offset_x", g_camera_offset[0], 0.0); - nh.param("camera_offset_y", g_camera_offset[1], 0.0); - nh.param("camera_offset_z", g_camera_offset[2], 0.0); -} - -inline void topicSub(ros::NodeHandle &nh) -{ - //【订阅】降落板与无人机的相对位置及相对偏航角 单位：米 单位：弧度 - // 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正） - // 标志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标 - static ros::Subscriber landpad_det_sub = nh.subscribe("/uav" + std::to_string(g_uav_id) + "/prometheus/object_detection/landpad_det", 10, landpadDetCb); - - // 无人机状态 - static ros::Subscriber drone_state_sub = nh.subscribe("/uav" + std::to_string(g_uav_id) + "/prometheus/state", 10, droneStateCb); - - // 地面真值，此信息仅做比较使用 不强制要求提供 - static ros::Subscriber groundtruth_sub = nh.subscribe("/ground_truth/landing_pad", 10, [&](const nav_msgs::Odometry::ConstPtr &msg) - { g_GroundTruth = *msg; }); - - // 订阅遥控器状态 - static ros::Subscriber uav_control_state_sub = nh.subscribe("/uav" + std::to_string(g_uav_id) + "/prometheus/control_state", 10, [&](const prometheus_msgs::UAVControlState::ConstPtr &msg) -> void - { g_uavcontrol_state = *msg; }); -} - -static ros::Publisher g_vision_switch_pub, g_command_pub; - -inline void topicAdv(ros::NodeHandle &nh) -{ - // 【发布】 视觉模块开关量 - g_vision_switch_pub = nh.advertise("/uav" + std::to_string(g_uav_id) + "/prometheus/switch/landpad_det", 10); - - //【发布】发送给控制模块命令 - g_command_pub = nh.advertise("/uav" + std::to_string(g_uav_id) + "/prometheus/command", 10); -} - -//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< -int main(int argc, char **argv) -{ - ros::init(argc, argv, "autonomous_landing"); - ros::NodeHandle nh("~"); - - // 节点运行频率： 30hz - ros::Rate rate(30.0); - - // 读取配置参数 - readParams(nh); - // 订阅话题 - topicSub(nh); - // 发布话题 - topicAdv(nh); - - g_command_now.Command_ID = 1; - exec_state = EXEC_STATE::WAITING; - while (ros::ok()) - { - //回调 - ros::spinOnce(); - // 等待进入COMMAND_CONTROL模式 - if (g_uavcontrol_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - PCOUT(-1, TAIL, "Waiting for enter COMMAND_CONTROL state"); - continue; - } - - switch (exec_state) - { - // 初始状态，等待视觉检测结果 - case WAITING: - { - if (g_landpad_det.is_detected) - { - exec_state = TRACKING; - break; - } - - // 发送视觉节点启动指令 - vision_switch.data = true; - g_vision_switch_pub.publish(vision_switch); - // 默认高度为2米，Modules/uav_control/launch/uav_control_outdoor.yaml - g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - PCOUT(-1, GREEN, "Waiting for the detection result."); - break; - } - // 追踪状态 - case TRACKING: - { - // 正常追踪 - char message_chars[256]; - sprintf(message_chars, "Tracking the Landing Pad, distance_to_the_pad : %f [m] .", g_distance_to_pad); - // 每1秒打印一次到pad的距离 - PCOUT(1, GREEN, message_chars); - - // 丢失,进入LOST状态 - if (!g_landpad_det.is_detected) - { - exec_state = LOST; - PCOUT(0, YELLOW, "Lost the Landing Pad."); - break; - } - - // 抵达上锁点,进入LANDING - g_distance_to_pad = g_landpad_det.pos_body_enu_frame.norm(); - //　达到降落距离，上锁降落 - if (g_distance_to_pad < g_arm_distance_to_pad) - { - exec_state = LANDING; - PCOUT(0, GREEN, "Catched the Landing Pad."); - break; - } - //　达到最低高度，上锁降落 - else if (abs(g_landpad_det.pos_body_enu_frame[2]) < g_arm_height_to_ground) - { - exec_state = LANDING; - PCOUT(0, GREEN, "Reach the lowest height."); - break; - } - - // 机体系速度控制 - g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Move; - g_command_now.Move_mode = prometheus_msgs::UAVCommand::XYZ_VEL; // xy velocity z position - - // 使用机体惯性系作为误差进行惯性系的速度控制 - for (int i = 0; i < 3; i++) - { - g_command_now.velocity_ref[i] = g_kp_land[i] * g_landpad_det.pos_body_enu_frame[i]; - } - // 动态调节下降速度，水平速度越大，垂直速度越小 - g_command_now.velocity_ref[2] = g_command_now.velocity_ref[2] / std::fmax(1., 10 * (g_command_now.velocity_ref[1] + g_command_now.velocity_ref[0])); - // 移动过程中，不调节航向角 - g_command_now.yaw_ref = 0.0; - - break; - } - // 目标丢失常识自动找回，在丢失目标后无人机先原定悬停一段时间，如果悬停到一段时候后 - // 仍然没有找到目标，则无人机持续向上飞行，到达一定高度后仍然未发现目标，判定自动 - // 降落任务失败，原点进入降落模式。 - case LOST: - { - static int lost_time = 0; - lost_time++; - - // 首先是悬停等待 尝试得到图像, 如果仍然获得不到图像 则原地上升 - if (lost_time < 10.0) - { - g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover; - - ros::Duration(0.4).sleep(); - } - else - { - g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Move; - g_command_now.Move_mode = prometheus_msgs::UAVCommand::XYZ_VEL; - g_command_now.velocity_ref[0] = 0.0; - g_command_now.velocity_ref[1] = 0.0; - g_command_now.velocity_ref[2] = 0.1; - g_command_now.yaw_ref = 0; - - // 如果上升超过原始高度，则认为任务失败，则直接降落 - if (g_UAVState.position[2] >= max_height) - { - exec_state = LANDING; - lost_time = 0; - PCOUT(0, RED, "Mission failed, landing... "); - } - } - - // 重新获得信息,进入TRACKING - if (g_landpad_det.is_detected) - { - exec_state = TRACKING; - PCOUT(0, GREEN, "Regain the Landing Pad."); - } - break; - } - case LANDING: - { - g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Land; - break; - } - } - g_command_now.header.stamp = ros::Time::now(); - g_command_now.Command_ID = g_command_now.Command_ID + 1; - g_command_pub.publish(g_command_now); - rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/find_aruco_marker/launch/find_aruco_marker_all.launch b/src/无人机端代码/Modules/tutorial_demo/advanced/find_aruco_marker/launch/find_aruco_marker_all.launch deleted file mode 100644 index d1b9e132..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/find_aruco_marker/launch/find_aruco_marker_all.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/find_aruco_marker/src/find_aruco_marker.cpp b/src/无人机端代码/Modules/tutorial_demo/advanced/find_aruco_marker/src/find_aruco_marker.cpp deleted file mode 100644 index 7eb53453..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/find_aruco_marker/src/find_aruco_marker.cpp +++ /dev/null @@ -1,272 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "printf_utils.h" -#include - -using namespace std; - -const float PI = 3.1415926; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; -prometheus_msgs::ArucoInfo now_arucos_info; -// 创建圆形跟踪的相关变量 -// 整个圆形的飞行时间 -float circular_time; -// 每次控制数据更新时的弧度增量 -float angle_increment; -// 无人机的合速度也就是圆的线速度 -float line_velocity; -// 无人机的控制频率 -float control_rate; -// 圆的半径 -float radius; -// 目标二维码id -int goto_id = 1; -// 最大目标丢失计数 -constexpr int max_loss_count = 30; -int loss_count = max_loss_count; - -//通过设定整个圆的飞行时间,控制频率,圆的半径来获取相关参数 -void get_circular_property(float time, int rate, float radius) -{ - //计算角速度(rad/s) - float w = 2 * PI / time; - //计算线速度(m/s) - line_velocity = -radius * w; - //计算控制数据发布的弧度增量 - angle_increment = w / rate; -} - -// 为 -void visualFeedbackCallback(const prometheus_msgs::MultiArucoInfo::ConstPtr &msg) -{ - now_arucos_info.detected = false; - for (auto &aruco : msg->aruco_infos) - { - if (aruco.aruco_num != goto_id || !aruco.detected) - continue; - now_arucos_info = aruco; - // 相机坐标到机体坐标系x、y轴需要交换，并且方向相反。 - now_arucos_info.position[0] = -aruco.position[1]; - now_arucos_info.position[1] = -aruco.position[0]; - // 0 0 0.05 0 1.5707963 0 - // 根据无人机pdf文件，下视觉相机在距离无人机质心0.05米位置 - // 无人机质心到地面距离是 0.09 - now_arucos_info.position[2] = -(aruco.position[2] + 0.05 - 0.09); - break; - } -} - -inline bool checkInput(int &goto_id) -{ - std::cout << "Please input the number(1~20) for that point to land on, input 0 exit program" << std::endl; - while (true) - { - try - { - std::cout << " > "; - std::cin >> goto_id; - if (goto_id == 0) - { - return false; - } - break; - } - catch (const std::exception &e) - { - } - std::cout << "Input number not in the range 1 to 20, re-input" << std::endl; - } - return true; -} - -//主函数 -int main(int argc, char **argv) -{ - // ROS初始化,设定节点名 - ros::init(argc, argv, "find_arcuo_marker"); - //创建句柄 - ros::NodeHandle n; - // 读取参数 - - int uav_id; - std::string visual_feedback_topic_name; - n.param("uav_id", uav_id, 1); - n.param("visual_feedback_topic_name", visual_feedback_topic_name, std::string("/uav1/prometheus/object_detection/arucos_det")); - - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 10); - //创建无人机状态命令订阅者，使用匿名函数,lambda作为回调函数 - ros::Subscriber uav_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", 10, [&](const prometheus_msgs::UAVState::ConstPtr &msg) - { uav_state = *msg; }); - //创建无人机控制状态命令订阅者，使用匿名函数,lambda作为回调函数 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", 10, [&](const prometheus_msgs::UAVControlState::ConstPtr &msg) - { uav_control_state = *msg; }); - - // 订阅视觉反馈 - ros::Subscriber visual_feedback_sub = n.subscribe(visual_feedback_topic_name, 20, visualFeedbackCallback); - - // 圆轨迹周期 - circular_time = 30; - control_rate = 60; - // 圆轨迹半径 - radius = 5; - - // 获取线速度line_velocity， 角速度angle_increment - get_circular_property(circular_time, control_rate, radius); - - bool circular_success = false; - int count = 0; - ros::Rate rate(control_rate); - - Eigen::Vector3d target_pos = {-radius, 0.0, 2.0}; - - // 到达那个二维码上方 - if (!checkInput(goto_id)) - return 0; - - enum STATUS - { - // 等到到达预设初始点 - WAITE_INIT_POS, - // 绕圆飞行 - CIRCULAR_SEARCH, - // 到达指定二维码上方 - TO_DESTINATION, - // 降落 - LAND, - }; - - STATUS run_status = WAITE_INIT_POS; - - while (ros::ok()) - { - prometheus_msgs::ArucoInfo now_marker; - - //调用一次回调函数 - ros::spinOnce(); - //检测无人机是否处于[COMMAND_CONTROL]模式 - if (uav_control_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - PCOUT(-1, WHITE, "Waiting for enter COMMAND_CONTROL mode"); - continue; - } - - std::ostringstream info; - - uav_command.header.stamp = ros::Time::now(); - switch (run_status) - { - // 无人机到达预设位置 - case WAITE_INIT_POS: - //坐标系 - uav_command.header.frame_id = "ENU"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - //无人机将会以当前位置移动 - uav_command.position_ref[0] = target_pos[0]; - uav_command.position_ref[1] = target_pos[1]; - uav_command.position_ref[2] = target_pos[2]; - uav_command.yaw_ref = 0.0; - - //打印无人机控制命令发布信息 - PCOUT(1, GREEN, "Go to initial point"); - - //检测无人机是否到达初始点 - //当无人机距离目标位置±0.1米范围内时认为到达初始点 - { - Eigen::Vector3d uav_pos = {uav_state.position[0], uav_state.position[1], uav_state.position[2]}; - float distance = (uav_pos - target_pos).norm(); - if (distance < 0.1) - { - PCOUT(1, GREEN, " UAV arrived at initial point"); - run_status = CIRCULAR_SEARCH; - } - } - break; - // 无人机绕圆飞行 - case CIRCULAR_SEARCH: - //坐标系 - uav_command.header.frame_id = "ENU"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS; - //无人机按照圆形轨迹飞行 - - uav_command.velocity_ref[0] = -line_velocity * std::sin(count * angle_increment); - uav_command.velocity_ref[1] = line_velocity * std::cos(count * angle_increment); - uav_command.velocity_ref[2] = 0; - uav_command.position_ref[2] = target_pos[2]; - //发布的命令ID加1 - //发布降落命令 - ++count; - info << "Waypoint Tracking > Velocity_x: " << uav_command.velocity_ref[0] << " Veloticy_y: " << uav_command.velocity_ref[1]; - PCOUT(1, GREEN, info.str()); - if (now_arucos_info.detected) - { - run_status = TO_DESTINATION; - loss_count = max_loss_count; - } - break; - // 前往二维码上方 - case TO_DESTINATION: - if (!now_arucos_info.detected) - { - --loss_count; - if (loss_count < 0) - run_status = CIRCULAR_SEARCH; - break; - } - //坐标系 - uav_command.header.frame_id = "BODY"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // 机体系下的速度控制 - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS_BODY; - - uav_command.velocity_ref[0] = 0.5 * now_arucos_info.position[0]; - uav_command.velocity_ref[1] = 0.5 * now_arucos_info.position[1]; - uav_command.position_ref[2] = target_pos[2]; - info << "Find object,Go to the target point > velocity_x: " << uav_command.velocity_ref[0] << " [m/s] " - << "velocity_y: " << uav_command.velocity_ref[1] << " [m/s] " - << std::endl; - PCOUT(1, GREEN, info.str()); - if (std::abs(uav_command.velocity_ref[0]) + std::abs(uav_command.velocity_ref[1]) < 0.04) - run_status = LAND; - break; - case LAND: - //坐标系 - uav_command.header.frame_id = "BODY"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // 机体系下的速度控制 - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_VEL_BODY; - uav_command.velocity_ref[0] = 0.5 * now_arucos_info.position[0]; - uav_command.velocity_ref[1] = 0.5 * now_arucos_info.position[1]; - uav_command.velocity_ref[2] = 0.2 * now_arucos_info.position[2]; - if (uav_state.position[2] < 0.4) - { - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - } - PCOUT(-1, GREEN, "LANDING..."); - break; - } - uav_command.Command_ID += 1; - uav_command_pub.publish(uav_command); - rate.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/formation_control/launch/formation_control.launch b/src/无人机端代码/Modules/tutorial_demo/advanced/formation_control/launch/formation_control.launch deleted file mode 100644 index 7c36a8ba..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/formation_control/launch/formation_control.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/formation_control/src/formation_control.cpp b/src/无人机端代码/Modules/tutorial_demo/advanced/formation_control/src/formation_control.cpp deleted file mode 100644 index 82858ded..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/formation_control/src/formation_control.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/****************************************************************************** -*例程简介: 无人机集群控制,包含无人机集群的模式控制/位置控制/队形变换/一字队形/三角队形 -* -*效果说明: - -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ - -#include -#include -#include -#include -#include -#include - -prometheus_msgs::GPSData origin_gps; - -// 输入参数：　阵型，阵型基本尺寸，集群数量 -// 所有的阵型和数量必须提前预设!! -// swarm_shape: 0代表一字队形,1代表三角队形 -Eigen::MatrixXf getFormationSeparation(int swarm_shape, float swarm_size, int swarm_num_uav) -{ - //矩阵大小为　swarm_num_uav＊4 , 对应x,y,z,yaw四个自由度的分离量 - Eigen::MatrixXf seperation(swarm_num_uav,4); - - //判断是否为奇数,位操作符&,如果为奇数,则2的0次方一定为1,那么奇数&1就等于1,偶数&1就等于0 - if((swarm_num_uav & 1) == 1) - { - int diff_ont_column_odd_value = (swarm_num_uav - 1) / 2; - int diff_triangle_odd_value = (swarm_num_uav - 1) / 2; - switch(swarm_shape) - { - //一字队形 - case 0: - for(int i = 0; i < swarm_num_uav; i++) - { - seperation(i,0) = 0.0; - seperation(i,1) = i - diff_ont_column_odd_value; - seperation(i,2) = 0.0; - } - break; - - //三角队形 - case 1: - for(int i = 0; i <=diff_triangle_odd_value; i++) - { - seperation(i,0) = i; - seperation(i,1) = i - diff_triangle_odd_value; - seperation(i,2) = 0.0; - - seperation(swarm_num_uav - 1 - i,0) = i; - seperation(swarm_num_uav - 1 - i,1) = diff_triangle_odd_value - i; - seperation(swarm_num_uav - 1 - i,2) = 0.0; - } - break; - } - } - else - { - float diff_one_column_even_value = swarm_num_uav / 2.0 - 0.5; - int diff_triangle_even_value = swarm_num_uav / 2; - switch (swarm_shape) - { - //一字队形 - case 0: - for(int i = 0; i < swarm_num_uav; i++) - { - seperation(i,0) = 0.0; - seperation(i,1) = i - diff_one_column_even_value; - seperation(i,2) = 0.0; - } - break; - - //三角队形 - case 1: - for(int i = 0; i < diff_triangle_even_value; i++) - { - seperation(i,0) = i; - seperation(i,1) = i + 1 - diff_triangle_even_value; - seperation(i,2) = 0.0; - - if(i+1 == diff_triangle_even_value) - { - seperation(swarm_num_uav - 1 - i,0) = 0; - } - else - { - seperation(swarm_num_uav - 1 - i,0) = i; - } - seperation(swarm_num_uav - 1 - i,1) = diff_triangle_even_value - i - 1; - seperation(swarm_num_uav - 1 - i,2) = 0.0; - } - break; - } - } - - for(int i = 0 ; i < swarm_num_uav ; i++) - { - for(int j = 0 ; j < 2; j++) - { - seperation(i,j) *= swarm_size; - } - } - - return seperation; -} - -void uav1_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - origin_gps.latitude = msg->latitude; - origin_gps.longitude = msg->longitude; - origin_gps.altitude = msg->altitude; -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "formation_control"); - ros::NodeHandle n("~"); - int agent_num; - int formation_size; - n.param("agent_num", agent_num, 3); - n.param("formation_size", formation_size, 1); - - int formation_shape; - Eigen::Vector3d leader_pos; - Eigen::MatrixXf separation = getFormationSeparation(0, formation_size, agent_num); - - ros::Publisher uav_command_pub[agent_num]; - ros::Publisher set_local_pose_offset_pub[agent_num]; - ros::Subscriber uav1_state_sub = n.subscribe("/uav1/prometheus/state", 1, uav1_state_cb); - - for(int i=0; i("/uav" + std::to_string(i+1) + "/prometheus/command", 10); - set_local_pose_offset_pub[i] = n.advertise("/uav" + std::to_string(i+1) + "/prometheus/set_local_offset_pose", 10); - } - - //此处做一个阻塞确保无人机全部正常启动后,能够将无人机原点坐标系统一到1号无人机所在位置 - std::cout << "Please input 1 to continue" << std::endl; - int start_flag; - std::cin >>start_flag; - ros::spinOnce(); - for(int i=0; i> control_command; - if(control_command == 0) - { - std::cout << "Please input X [m]:" << std::endl; - std::cin >> leader_pos[0]; - - std::cout << "Please input Y [m]:" << std::endl; - std::cin >> leader_pos[1]; - - std::cout << "Please input Z [m]:" << std::endl; - std::cin >> leader_pos[2]; - }else if(control_command == 1) - { - separation = getFormationSeparation(0, formation_size, agent_num); - }else if(control_command == 2) - { - separation = getFormationSeparation(1, formation_size, agent_num); - } - - for(int i=0; i - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/siamrpn_track/src/siamrpn_track.cpp b/src/无人机端代码/Modules/tutorial_demo/advanced/siamrpn_track/src/siamrpn_track.cpp deleted file mode 100644 index 7e4679aa..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/siamrpn_track/src/siamrpn_track.cpp +++ /dev/null @@ -1,176 +0,0 @@ -// ros头文件 -#include -#include -#include -#include - -// topic 头文件 -#include -#include -#include -#include - -#include "printf_utils.h" - -using namespace std; -using namespace Eigen; -#define NODE_NAME "object_tracking" -//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< -prometheus_msgs::UAVState g_UAVState; -Eigen::Vector3f g_drone_pos; -//---------------------------------------Vision--------------------------------------------- -prometheus_msgs::DetectionInfo g_Detection_raw; //目标位置[机体系下：前方x为正，右方y为正，下方z为正] -prometheus_msgs::UAVControlState g_uavcontrol_state; //目标位置[机体系下：前方x为正，右方y为正，下方z为正] -Eigen::Vector3f pos_body_frame; -Eigen::Vector3f pos_body_enu_frame; //原点位于质心，x轴指向前方，y轴指向左，z轴指向上的坐标系 -float kpx_track, kpy_track, kpz_track; //控制参数 - 比例参数 -bool is_detected = false; // 是否检测到目标标志 -int num_count_vision_lost = 0; //视觉丢失计数器 -int num_count_vision_regain = 0; //视觉丢失计数器 -int g_uav_id; -int Thres_vision = 0; //视觉丢失计数器阈值 -Eigen::Vector3f camera_offset; -//---------------------------------------Track--------------------------------------------- -float distance_to_setpoint; -Eigen::Vector3f tracking_delta; -//---------------------------------------Output--------------------------------------------- -prometheus_msgs::UAVCommand g_command_now; //发送给控制模块 [px4_pos_controller.cpp]的命令 -//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< -void vision_cb(const prometheus_msgs::DetectionInfo::ConstPtr &msg) -{ - g_Detection_raw = *msg; - - pos_body_frame[0] = g_Detection_raw.position[2] + camera_offset[0]; - pos_body_frame[1] = -g_Detection_raw.position[0] + camera_offset[1]; - pos_body_frame[2] = -g_Detection_raw.position[1] + camera_offset[2]; - - Eigen::Matrix3f R_Body_to_ENU; - - R_Body_to_ENU = get_rotation_matrix(g_UAVState.attitude[0], g_UAVState.attitude[1], g_UAVState.attitude[2]); - - pos_body_enu_frame = R_Body_to_ENU * pos_body_frame; - - if (g_Detection_raw.detected) - { - num_count_vision_regain++; - num_count_vision_lost = 0; - } - else - { - num_count_vision_regain = 0; - num_count_vision_lost++; - } - - // 当连续一段时间无法检测到目标时，认定目标丢失 - if (num_count_vision_lost > Thres_vision) - { - is_detected = false; - } - - // 当连续一段时间检测到目标时，认定目标得到 - if (num_count_vision_regain > Thres_vision) - { - is_detected = true; - } -} -void drone_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - g_UAVState = *msg; - - g_drone_pos[0] = g_UAVState.position[0]; - g_drone_pos[1] = g_UAVState.position[1]; - g_drone_pos[2] = g_UAVState.position[2]; -} - -//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< -int main(int argc, char **argv) -{ - ros::init(argc, argv, "object_tracking"); - ros::NodeHandle nh("~"); - - //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< - //节点运行频率： 20hz 【视觉端解算频率大概为20HZ】 - ros::Rate rate(20.0); - - //视觉丢失次数阈值 - nh.param("Thres_vision", Thres_vision, 10); - - //追踪的前后间隔 - nh.param("tracking_delta_x", tracking_delta[0], 2); - nh.param("tracking_delta_y", tracking_delta[1], 0.0); - nh.param("tracking_delta_z", tracking_delta[2], 2); - - nh.param("camera_offset_x", camera_offset[0], 0.0); - nh.param("camera_offset_y", camera_offset[1], 0.0); - nh.param("camera_offset_z", camera_offset[2], 0.0); - - //追踪控制参数 - nh.param("kpx_track", kpx_track, 0.1); - nh.param("kpy_track", kpy_track, 0.1); - nh.param("kpz_track", kpz_track, 0.1); - - nh.param("uav_id", g_uav_id, 1); - - // 订阅视觉反馈 - ros::Subscriber vision_sub = nh.subscribe("/uav" + std::to_string(g_uav_id) + "/prometheus/object_detection/siamrpn_tracker", 10, vision_cb); - // 订阅当前无人机控制状态, 使用lambda匿名函数 - ros::Subscriber uav_control_state_sub = nh.subscribe("/uav" + std::to_string(g_uav_id) + "/prometheus/control_state", 10, [&](const prometheus_msgs::UAVControlState::ConstPtr &msg) -> void - { g_uavcontrol_state = *msg; }); - // 订阅无人机状态 - ros::Subscriber drone_state_sub = nh.subscribe("/uav" + std::to_string(g_uav_id) + "/prometheus/state", 10, drone_state_cb); - - // 发布控制指令 - ros::Publisher command_pub = nh.advertise("/uav" + std::to_string(g_uav_id) + "/prometheus/command", 10); - - // 初始化 - g_command_now.position_ref[0] = 0; - g_command_now.position_ref[1] = 0; - g_command_now.position_ref[2] = 0; - - bool is_inited = false; - - while (ros::ok()) - { - // 执行回调获取数据 - ros::spinOnce(); - if (g_uavcontrol_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - PCOUT(-1, TAIL, "Waiting for enter COMMAND_CONTROL state"); - continue; - } - - if (!is_detected) - { - g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover; - PCOUT(-1, GREEN, "Waiting for detected object"); - } - else - { - // 到目标的直线距离(估计值) - distance_to_setpoint = pos_body_frame.norm(); - - g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // 使用全速度控制 - g_command_now.Move_mode = prometheus_msgs::UAVCommand::XYZ_VEL; // xy velocity z position - - // 根据误差计算计算应该给于无人机的速度 - g_command_now.velocity_ref[0] = kpx_track * (pos_body_enu_frame[0] - tracking_delta[0]); - g_command_now.velocity_ref[1] = kpy_track * (pos_body_enu_frame[1] - tracking_delta[1]); - g_command_now.velocity_ref[2] = kpz_track * (tracking_delta[2] - g_UAVState.position[2]); - g_command_now.yaw_ref = 0; - std::string tmp = "[object_tracking]: Tracking the Target, distance_to_setpoint : " + std::to_string(distance_to_setpoint) + " [m] " + "\n velocity_x: " + std::to_string(g_command_now.velocity_ref[0]) + " [m/s], velocity_y: " + std::to_string(g_command_now.velocity_ref[1]) + " [m/s], velocity_y: " + std::to_string(g_command_now.velocity_ref[2]) + " [m/s]"; - PCOUT(1, GREEN, tmp); - } - - // Publish - g_command_now.header.stamp = ros::Time::now(); - g_command_now.Command_ID = g_command_now.Command_ID + 1; - if (g_command_now.Command_ID < 10) - g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - command_pub.publish(g_command_now); - - rate.sleep(); - } - - return 0; -} diff --git a/src/无人机端代码/Modules/tutorial_demo/advanced/yolov5_track/launch/yolov5_track_all.launch b/src/无人机端代码/Modules/tutorial_demo/advanced/yolov5_track/launch/yolov5_track_all.launch deleted file mode 100644 index c767ed17..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/advanced/yolov5_track/launch/yolov5_track_all.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/launch/body_xyz_pos_control.launch b/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/launch/body_xyz_pos_control.launch deleted file mode 100644 index bf3f9f58..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/launch/body_xyz_pos_control.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/scripts/body_xyz_pos_control.py b/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/scripts/body_xyz_pos_control.py deleted file mode 100644 index dfd78137..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/scripts/body_xyz_pos_control.py +++ /dev/null @@ -1,118 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# 例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的位置控制 -# 效果说明: 无人机起飞移动到目标位置点,悬停30秒后降落 -# 备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -from math import fabs -from turtle import position -import ros -import rospy -from prometheus_msgs.msg import UAVCommand, UAVControlState, UAVState - -# 创建无人机相关数据变量 -uav_control_state_sv = UAVControlState() -uav_command_pv = UAVCommand() -uav_state_sv = UAVState() - -# 无人机状态回调函数 -def uavStateCb(msg): - global uav_state_sv - uav_state_sv = msg - -# 无人机控制状态回调函数 -def uavControlStateCb(msg): - global uav_control_state_sv - uav_control_state_sv = msg - -# 主函数 -def main(): - # ROS初始化,设定节点名 - rospy.init_node('body_xyz_pos_control',anonymous=True) - # 创建命令发布标志位,命令发布则为true;初始化为false - cmd_pub_flag = False - # 创建无人机控制命令发布者 - UavCommandPb = rospy.Publisher("/uav1/prometheus/command", UAVCommand, queue_size =10) - # 创建无人机控制状态命令订阅者 - rospy.Subscriber("/uav1/prometheus/control_state", UAVControlState, uavControlStateCb) - # 创建无人机状态命令订阅者 - rospy.Subscriber("/uav1/prometheus/state", UAVState, uavStateCb) - # 循环频率设置为1HZ - rate = rospy.Rate(1) - while not rospy.is_shutdown(): - # 检测无人机是否处于[COMMAND_CONTROL]模式 - if uav_control_state_sv.control_state == UAVControlState.COMMAND_CONTROL: - # 检测控制命令是否发布,没有发布则进行命令的发布 - if not cmd_pub_flag: - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = 'ENU' - # Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度 - uav_command_pv.Agent_CMD = 1 - # 发布的命令ID,每发一次,该ID加1 - uav_command_pv.Command_ID = 1 - # 发布起飞命令 - UavCommandPb.publish(uav_command_pv) - rate.sleep() - # 命令发布标志位置为true - cmd_pub_flag = True - # 打印无人机起飞相关信息 - rospy.loginfo("Takeoff_height: %d", rospy.get_param('/uav_control_main_1/control/Takeoff_height')) - else: - # 当无人机距离高度目标值±0.1米范围内时认为起飞完 - if fabs(uav_state_sv.position[2] - rospy.get_param('/uav_control_main_1/control/Takeoff_height')) <= 0.1: - print(" UAV takeoff successfully and move body position control") - rospy.sleep(5) - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = "BODY" - # Move模式 - uav_command_pv.Agent_CMD = UAVCommand.Move - # Move_mode - uav_command_pv.Move_mode = UAVCommand.XYZ_POS_BODY - # BODY坐标系下的X轴正半轴对应东方,Y轴正半轴对应北方,因此下面的控制数据将会控制无人机在5米的高度移动(2,2,2) - uav_command_pv.position_ref[0] = 2 - uav_command_pv.position_ref[1] = 2 - uav_command_pv.position_ref[2] = 2 - uav_command_pv.yaw_ref = 0 - # 发布的命令ID加1 - uav_command_pv.Command_ID += 1 - # 发布命令 - UavCommandPb.publish(uav_command_pv) - rospy.loginfo("UAV move body position control and landed after 10 seconds") - rospy.sleep(10) - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = "ENU" - # Land降落,从当前位置降落至地面并自动上锁 - uav_command_pv.Agent_CMD = 3 - # 发布的命令ID加1 - uav_command_pv.Command_ID += 1 - # 发布降落命令 - UavCommandPb.publish(uav_command_pv) - # 打印降落相关信息 - rospy.loginfo("[body_xyz_pos_control] tutorial_demo completed") - # 任务结束,关闭该节点 - rospy.signal_shutdown("shutdown time") - else: - # 打印当前无人机高度信息 - rospy.loginfo("UAV height : %f [m]", uav_state_sv.position[2]) - rospy.sleep(1) - else: - # 在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if cmd_pub_flag: - rospy.logfatal(" Unknown error! [body_xyz_pos_control] tutorial_demo aborted") - # 命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else: - rospy.logwarn(" Wait for UAV to enter [COMMAND_CONTROL] MODE ") - rospy.sleep(2) - rate.sleep() - rospy.spin() - -if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/src/body_xyz_pos_control.cpp b/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/src/body_xyz_pos_control.cpp deleted file mode 100644 index 54bf16ce..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/body_xyz_pos_control/src/body_xyz_pos_control.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/****************************************************************************** -*例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的位置控制 -* -*效果说明: 无人机起飞移动到目标位置点,悬停30秒后降落 -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include "printf_utils.h" - -using namespace std; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; -Eigen::Vector3d target_pos = {1.0, 0.0, 0}; - -//无人机状态回调函数 -void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; -} - -//无人机控制状态回调函数 -void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -//主函数 -int main(int argc, char** argv) -{ - //ROS初始化,设定节点名 - ros::init(argc , argv, "body_xyz_pos_control"); - //创建句柄 - ros::NodeHandle n; - - //声明起飞高度,无人机id变量 - float takeoff_height; - int uav_id; - //获取起飞高度参数 - ros::param::get("/uav_control_main_1/control/Takeoff_height", takeoff_height); - ros::param::get("~uav_id", uav_id); - - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 10); - //创建无人机状态命令订阅者 - ros::Subscriber uav_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", 10, uav_state_cb); - //创建无人机控制状态命令订阅者 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", 10, uav_control_state_cb); - //循环频率设置为1HZ - ros::Rate r(1); - //创建命令发布标志位,命令发布则为true;初始化为false - bool cmd_pub_flag = false; - - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为2位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - - //打印demo相关信息 - cout << GREEN << " [BODY XYZ position control] tutorial_demo start " << TAIL << endl; - sleep(1); - cout << GREEN << " Level: [Basic] " << TAIL << endl; - sleep(1); - cout << GREEN << " Please use the RC SWA to armed, and the SWB to switch the drone to [COMMAND_CONTROL] mode " << TAIL << endl; - - while(ros::ok()) - { - //调用一次回调函数 - ros::spinOnce(); - //检测无人机是否处于[COMMAND_CONTROL]模式 - if(uav_control_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - //检测控制命令是否发布,没有发布则进行命令的发布 - if(!cmd_pub_flag) - { - //检测无人机是否到达预设起飞高度 - if(fabs(uav_state.position[2] - takeoff_height) >= 0.3) - { - continue; - } - cout << GREEN << " UAV takeoff success" << TAIL << endl; - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "BODY"; - //Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - //Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS_BODY; - //无人机将会以当前位置移动 - uav_command.position_ref[0] = target_pos[0]; - uav_command.position_ref[1] = target_pos[1]; - uav_command.position_ref[2] = target_pos[2]; - uav_command.yaw_ref = 0.0; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //命令发布标志位置为true - cmd_pub_flag = true; - - //打印无人机控制命令发布信息 - cout << GREEN << " [XYZ_POS_BODY] command publish " << TAIL << endl; - - //因机体坐标系下的位置控制坐标系会以无人机当前位置为坐标原点,因此实际发布的位置目标点会加上当前位置数据 - target_pos[0] += uav_state.position[0]; - target_pos[1] += uav_state.position[1]; - target_pos[2] += uav_state.position[2]; - } - else - { - //当无人机距离目标位置±0.3米范围内时认为任务完成并等待30秒后降落 - Eigen::Vector3d uav_pos = {uav_state.position[0], uav_state.position[1], uav_state.position[2]}; - float distance = (uav_pos - target_pos).norm(); - if(distance <= 0.3) - { - cout << GREEN << " UAV arrived at target position and landed after 30 seconds" << TAIL << endl; - sleep(30); - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "BODY"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //打印降落相关信息 - cout << GREEN << " UAV Land" << TAIL << endl; - cout << GREEN << " [BODY XYZ position control] tutorial_demo completed" << TAIL << endl; - //任务结束,关闭该节点 - ros::shutdown(); - } - else - { - //打印信息 - cout << GREEN << " [ " << distance << " ] m to the target point " << TAIL << endl; - } - } - } - else - { - //在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if(cmd_pub_flag) - { - cout << RED << " Unknown error! [BODY XYZ position control] tutorial_demo aborted" << TAIL << endl; - } - //命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else - { - cout << YELLOW << " Wait for UAV to enter [COMMAND_CONTROL] MODE " << TAIL << endl; - } - } - r.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/launch/circular_trajectory_control.launch b/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/launch/circular_trajectory_control.launch deleted file mode 100644 index 66bae266..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/launch/circular_trajectory_control.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/scripts/circular_trajectory_control.py b/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/scripts/circular_trajectory_control.py deleted file mode 100644 index 00757ebf..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/scripts/circular_trajectory_control.py +++ /dev/null @@ -1,152 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# 例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的XY速度Z位置控制以及轨迹控制 -# 效果说明: 无人机起飞后开始按照圆的轨迹飞行,飞行结束后悬停30秒,然后降落 -# 备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -from itertools import count -from math import cos, fabs, sin -import math -from mimetypes import init -from time import time -import ros -import rospy -from prometheus_msgs.msg import UAVCommand, UAVControlState, UAVState -import numpy as np - -# 创建无人机相关数据变量 -uav_control_state_sv = UAVControlState() -uav_command_pv = UAVCommand() -uav_state_sv = UAVState() -count = 0 -Pi = 3.1415926 -# 创建圆形跟踪的相关变量 -# 整个圆形的飞行时间 -circular_time = 40 -# 无人机的控制频率 -control_rate = 20 -# 圆的半径 -radius = 2 - - -def get_circular_property(circular_time,control_rate,radius): - # 圆的角速度,圆的线速度,每次控制数据更新时的弧度增量 - global w,velocity,angle_increment - # 计算角速度(rad/s) - w = (2 * Pi) / circular_time - # 计算线速度(m/s) - velocity = radius * w - # 计算控制数据发布的弧度增量 - angle_increment = w / control_rate - -# 无人机状态回调函数 -def uavStateCb(msg): - global uav_state_sv - uav_state_sv = msg - -# 无人机控制状态回调函数 -def uavControlStateCb(msg): - global uav_control_state_sv - uav_control_state_sv = msg - -# 主函数 -def main(): - # ROS初始化,设定节点名 - rospy.init_node('circular_trajectory_control',anonymous=True) - # 创建命令发布标志位,命令发布则为true;初始化为false - cmd_pub_flag = False - # 创建无人机控制命令发布者 - UavCommandPb = rospy.Publisher("/uav1/prometheus/command", UAVCommand, queue_size =10) - # 创建无人机控制状态命令订阅者 - rospy.Subscriber("/uav1/prometheus/control_state", UAVControlState, uavControlStateCb) - # 创建无人机状态命令订阅者 - rospy.Subscriber("/uav1/prometheus/state", UAVState, uavStateCb) - # 循环频率设置为20HZ - rate = rospy.Rate(control_rate) - while not rospy.is_shutdown(): - # 检测无人机是否处于[COMMAND_CONTROL]模式 - if uav_control_state_sv.control_state == UAVControlState.COMMAND_CONTROL: - # 检测控制命令是否发布,没有发布则进行命令的发布 - if not cmd_pub_flag: - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = 'ENU' - # Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度 - uav_command_pv.Agent_CMD = 1 - # 发布的命令ID,每发一次,该ID加1 - uav_command_pv.Command_ID = 1 - # 发布起飞命令 - UavCommandPb.publish(uav_command_pv) - rate.sleep() - # 打印无人机起飞相关信息 - rospy.loginfo("Takeoff_height: %d", rospy.get_param('/uav_control_main_1/control/Takeoff_height')) - # 当无人机距离高度目标值±0.1米范围内时认为起飞完,检测无人机是否到达初始点 - if fabs(uav_state_sv.position[2] - rospy.get_param('/uav_control_main_1/control/Takeoff_height')) <= 0.1: - print(" UAV takeoff successfully and arrived at initial point") - rospy.sleep(5) - # 命令发布标志位置为true - cmd_pub_flag = True - else: - # # 打印当前无人机高度信息 - rospy.loginfo("UAV height : %f [m]", uav_state_sv.position[2]) - rospy.sleep(1) - else: - initial_point_success = False - circular_success = False - global count - get_circular_property(circular_time,control_rate,radius) - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = "ENU" - # Move模式 - uav_command_pv.Agent_CMD = UAVCommand.Move - # Move_mode - uav_command_pv.Move_mode = UAVCommand.XYZ_VEL - # 无人机按照圆形轨迹飞行 - uav_command_pv.velocity_ref[0] = - velocity * sin(count * angle_increment) - uav_command_pv.velocity_ref[1] = velocity * cos(count * angle_increment) - uav_command_pv.velocity_ref[2] = 0 - # 发布的命令ID加1 - uav_command_pv.Command_ID += 1 - # 发布命令 - UavCommandPb.publish(uav_command_pv) - # 计数器 - count = count + 1 - if count == control_rate * circular_time: - circular_success = True - rospy.loginfo(" count: %d ",count) - rospy.loginfo(" Vx: %f ",uav_command_pv.velocity_ref[0]) - rospy.loginfo(" Vy: %f ",uav_command_pv.velocity_ref[1]) - if circular_success: - rospy.loginfo("UAV circular trajectory completed and landed after 10 seconds") - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = "ENU" - # Land降落,从当前位置降落至地面并自动上锁 - uav_command_pv.Agent_CMD = 3 - # 发布的命令ID加1 - uav_command_pv.Command_ID += 1 - # 发布降落命令 - UavCommandPb.publish(uav_command_pv) - # 打印降落相关信息 - rospy.loginfo("[circular trajectory control] tutorial_demo completed") - # 任务结束,关闭该节点 - rospy.signal_shutdown("shutdown time") - else: - # 在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if cmd_pub_flag: - rospy.logfatal(" Unknown error! [body_xyz_pos_control] tutorial_demo aborted") - # 命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else: - rospy.logwarn(" Wait for UAV to enter [COMMAND_CONTROL] MODE ") - rospy.sleep(2) - rate.sleep() - rospy.spin() - -if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/src/circular_trajectory_control.cpp b/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/src/circular_trajectory_control.cpp deleted file mode 100644 index 47792004..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/circular_trajectory_control/src/circular_trajectory_control.cpp +++ /dev/null @@ -1,247 +0,0 @@ -/****************************************************************************** -*例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的XY速度Z位置控制以及 -* 轨迹控制 -* -*效果说明: 无人机起飞后开始按照圆的轨迹飞行,飞行结束后悬停30秒,然后降落 -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ - - -#include -#include -#include -#include -#include -#include -#include -#include "printf_utils.h" - -using namespace std; - -const float PI = 3.1415926; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; -//创建圆形跟踪的相关变量 -//整个圆形的飞行时间 -float circular_time; -//每次控制数据更新时的弧度增量 -float angle_increment; -//无人机的合速度也就是圆的线速度 -float velocity; -//无人机的控制频率 -float control_rate; -//圆的半径 -float radius; - -//通过设定整个圆的飞行时间,控制频率,圆的半径来获取相关参数 -void get_circular_property(float time, int rate, float radius) -{ - //计算角速度(rad/s) - float w = 2*PI/time; - //计算线速度(m/s) - velocity = radius * w; - //计算控制数据发布的弧度增量 - angle_increment = w/rate; -} - - - -//无人机状态回调函数 -void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; -} - -//无人机控制状态回调函数 -void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -//主函数 -int main(int argc, char** argv) -{ - //ROS初始化,设定节点名 - ros::init(argc , argv, "body_xyz_pos_control"); - //创建句柄 - ros::NodeHandle n; - //获取无人机id - int uav_id; - ros::param::get("~uav_id", uav_id); - - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 10); - //创建无人机状态命令订阅者 - ros::Subscriber uav_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", 10, uav_state_cb); - //创建无人机控制状态命令订阅者 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", 10, uav_control_state_cb); - //循环频率设置为1HZ - ros::Rate r(1); - //创建命令发布标志位,命令发布则为true;初始化为false - bool cmd_pub_flag = false; - - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为2位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - - //打印demo相关信息 - cout << GREEN << " [circular trajectory control] tutorial_demo start " << TAIL << endl; - sleep(1); - cout << GREEN << " Level: [Basic] " << TAIL << endl; - sleep(1); - cout << GREEN << " Please use the RC SWA to armed, and the SWB to switch the drone to [COMMAND_CONTROL] mode " << TAIL << endl; - - //设定飞行时间为40S,控制频率为20HZ,半径为2米 - circular_time = 40; - control_rate = 20; - radius = 2; - - get_circular_property(circular_time, control_rate, radius); - - bool circular_success = false; - int count = 0; - ros::Rate rate(control_rate); - - Eigen::Vector3d target_pos = {radius, 0.0, 2.0}; - - while(ros::ok()) - { - //调用一次回调函数 - ros::spinOnce(); - //检测无人机是否处于[COMMAND_CONTROL]模式 - if(uav_control_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - //检测控制命令是否发布,没有发布则进行命令的发布 - if(!cmd_pub_flag) - { - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - //Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - //无人机将会以当前位置移动 - uav_command.position_ref[0] = target_pos[0]; - uav_command.position_ref[1] = target_pos[1]; - uav_command.position_ref[2] = target_pos[2]; - uav_command.yaw_ref = 0.0; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //命令发布标志位置为true - cmd_pub_flag = true; - - //打印无人机控制命令发布信息 - cout << GREEN << " Go to initial point " << TAIL << endl; - - //检测无人机是否到达初始点 - bool initial_point_success = false; - while(!initial_point_success) - { - ros::spinOnce(); - //当无人机距离目标位置±0.1米范围内时认为到达初始点 - Eigen::Vector3d uav_pos = {uav_state.position[0], uav_state.position[1], uav_state.position[2]}; - float distance = (uav_pos - target_pos).norm(); - if(distance < 0.1) - { - cout << GREEN << " UAV arrived at initial point " << TAIL << endl; - initial_point_success = true; - } - r.sleep(); - } - } - else - { - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - //Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS; - //无人机按照圆形轨迹飞行 - uav_command.velocity_ref[0] = -velocity*sin(count*angle_increment); - uav_command.velocity_ref[1] = velocity*cos(count*angle_increment); - uav_command.velocity_ref[2] = 0; - uav_command.position_ref[2] = target_pos[2]; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //计数器 - if(count == control_rate*circular_time) - { - circular_success = true; - } - count++; - cout << GREEN << " Vx: " << uav_command.velocity_ref[0] << TAIL << endl; - cout << GREEN << " Vy: " << uav_command.velocity_ref[1] << TAIL << endl; - - if(circular_success) - { - cout << GREEN << " UAV circular trajectory completed and landed after 30 seconds" << TAIL << endl; - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - sleep(30); - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //打印降落相关信息 - cout << GREEN << " UAV Land" << TAIL << endl; - cout << GREEN << " [circular trajectory control] tutorial_demo completed" << TAIL << endl; - //任务结束,关闭该节点 - ros::shutdown(); - } - } - } - else - { - //在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if(cmd_pub_flag) - { - cout << RED << " Unknown error! [ENU XYZ position control] tutorial_demo aborted" << TAIL << endl; - r.sleep(); - continue; - } - //命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else - { - cout << YELLOW << " Wait for UAV to enter [COMMAND_CONTROL] MODE " << TAIL << endl; - r.sleep(); - continue; - } - } - rate.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/launch/enu_xyz_pos_control.launch b/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/launch/enu_xyz_pos_control.launch deleted file mode 100644 index fdf496e0..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/launch/enu_xyz_pos_control.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/scripts/enu_xyz_pos_control.py b/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/scripts/enu_xyz_pos_control.py deleted file mode 100644 index 436cf448..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/scripts/enu_xyz_pos_control.py +++ /dev/null @@ -1,117 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# 例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的位置控制 -# 效果说明: 无人机起飞移动到目标位置点,悬停30秒后降落 -# 备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -from math import fabs -import ros -import rospy -from prometheus_msgs.msg import UAVCommand, UAVControlState, UAVState - -# 创建无人机相关数据变量 -uav_control_state_sv = UAVControlState() -uav_command_pv = UAVCommand() -uav_state_sv = UAVState() - -# 无人机状态回调函数 -def uavStateCb(msg): - global uav_state_sv - uav_state_sv = msg - -# 无人机控制状态回调函数 -def uavControlStateCb(msg): - global uav_control_state_sv - uav_control_state_sv = msg - -# 主函数 -def main(): - # ROS初始化,设定节点名 - rospy.init_node('enu_xyz_pos_control',anonymous=True) - # 创建命令发布标志位,命令发布则为true;初始化为false - cmd_pub_flag = False - # 创建无人机控制命令发布者 - UavCommandPb = rospy.Publisher("/uav1/prometheus/command", UAVCommand, queue_size =10) - # 创建无人机控制状态命令订阅者 - rospy.Subscriber("/uav1/prometheus/control_state", UAVControlState, uavControlStateCb) - # 创建无人机状态命令订阅者 - rospy.Subscriber("/uav1/prometheus/state", UAVState, uavStateCb) - # 循环频率设置为1HZ - rate = rospy.Rate(1) - while not rospy.is_shutdown(): - # 检测无人机是否处于[COMMAND_CONTROL]模式 - if uav_control_state_sv.control_state == UAVControlState.COMMAND_CONTROL: - # 检测控制命令是否发布,没有发布则进行命令的发布 - if not cmd_pub_flag: - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = 'ENU' - # Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度 - uav_command_pv.Agent_CMD = 1 - # 发布的命令ID,每发一次,该ID加1 - uav_command_pv.Command_ID = 1 - # 发布起飞命令 - UavCommandPb.publish(uav_command_pv) - rate.sleep() - # 命令发布标志位置为true - cmd_pub_flag = True - # 打印无人机起飞相关信息 - rospy.loginfo("Takeoff_height: %d", rospy.get_param('/uav_control_main_1/control/Takeoff_height')) - else: - # 当无人机距离高度目标值±0.1米范围内时认为起飞完 - if fabs(uav_state_sv.position[2] - rospy.get_param('/uav_control_main_1/control/Takeoff_height')) <= 0.1: - print(" UAV takeoff successfully and move body position control") - rospy.sleep(5) - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = "ENU" - # Move模式 - uav_command_pv.Agent_CMD = UAVCommand.Move - # Move_mode - uav_command_pv.Move_mode = UAVCommand.XYZ_POS - # ENU坐标系下的X轴正半轴对应东方,Y轴正半轴对应北方,因此下面的控制数据将会控制无人机在5米的高度移动(3,4,5) - uav_command_pv.position_ref[0] = 3 - uav_command_pv.position_ref[1] = 4 - uav_command_pv.position_ref[2] = 5 - uav_command_pv.yaw_ref = 0 - # 发布的命令ID加1 - uav_command_pv.Command_ID += 1 - # 发布命令 - UavCommandPb.publish(uav_command_pv) - rospy.loginfo("UAV move body position control and landed after 10 seconds") - rospy.sleep(10) - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = "ENU" - # Land降落,从当前位置降落至地面并自动上锁 - uav_command_pv.Agent_CMD = 3 - # 发布的命令ID加1 - uav_command_pv.Command_ID += 1 - # 发布降落命令 - UavCommandPb.publish(uav_command_pv) - # 打印降落相关信息 - rospy.loginfo("[body_xyz_pos_control] tutorial_demo completed") - # 任务结束,关闭该节点 - rospy.signal_shutdown("shutdown time") - else: - # 打印当前无人机高度信息 - rospy.loginfo("UAV height : %f [m]", uav_state_sv.position[2]) - rospy.sleep(1) - else: - # 在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if cmd_pub_flag: - rospy.logfatal(" Unknown error! [body_xyz_pos_control] tutorial_demo aborted") - # 命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else: - rospy.logwarn(" Wait for UAV to enter [COMMAND_CONTROL] MODE ") - rospy.sleep(2) - rate.sleep() - rospy.spin() - -if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/src/enu_xyz_pos_control.cpp b/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/src/enu_xyz_pos_control.cpp deleted file mode 100644 index 5a8666ad..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/enu_xyz_pos_control/src/enu_xyz_pos_control.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/****************************************************************************** -*例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的位置控制 -* -*效果说明: 无人机起飞移动到目标位置点,悬停30秒后降落 -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include "printf_utils.h" - -using namespace std; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; -Eigen::Vector3d target_pos = {1.0, 0.0, 1.0}; - -//无人机状态回调函数 -void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; -} - -//无人机控制状态回调函数 -void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -//主函数 -int main(int argc, char** argv) -{ - //ROS初始化,设定节点名 - ros::init(argc , argv, "enu_xyz_pos_control"); - //创建句柄 - ros::NodeHandle n; - - //声明起飞高度,无人机id变量 - float takeoff_height; - int uav_id; - //获取起飞高度参数 - ros::param::get("/uav_control_main_1/control/Takeoff_height", takeoff_height); - ros::param::get("~uav_id", uav_id); - - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 10); - //创建无人机状态命令订阅者 - ros::Subscriber uav_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", 10, uav_state_cb); - //创建无人机控制状态命令订阅者 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", 10, uav_control_state_cb); - //循环频率设置为1HZ - ros::Rate r(1); - //创建命令发布标志位,命令发布则为true;初始化为false - bool cmd_pub_flag = false; - - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为2位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - - //打印demo相关信息 - cout << GREEN << " [ENU XYZ position control] tutorial_demo start " << TAIL << endl; - sleep(1); - cout << GREEN << " Level: [Basic] " << TAIL << endl; - sleep(1); - cout << GREEN << " Please use the RC SWA to armed, and the SWB to switch the drone to [COMMAND_CONTROL] mode " << TAIL << endl; - - while(ros::ok()) - { - //调用一次回调函数 - ros::spinOnce(); - //检测无人机是否处于[COMMAND_CONTROL]模式 - if(uav_control_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - //检测控制命令是否发布,没有发布则进行命令的发布 - if(!cmd_pub_flag) - { - //检测无人机是否到达预设起飞高度 - if(fabs(uav_state.position[2] - takeoff_height) >= 0.3) - { - continue; - } - cout << GREEN << " UAV takeoff success" << TAIL << endl; - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - //Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - //ENU坐标系下的X轴正半轴对应东方,Y轴正半轴对应北方,因此下面的控制数据将会控制无人机在5米的高度向东边移动一米 - uav_command.position_ref[0] = target_pos[0]; - uav_command.position_ref[1] = target_pos[1]; - uav_command.position_ref[2] = target_pos[2]; - uav_command.yaw_ref = 0.0; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //命令发布标志位置为true - cmd_pub_flag = true; - - //打印无人机控制命令发布信息 - cout << GREEN << " [XYZ_POS] command publish " << TAIL << endl; - } - else - { - //当无人机距离目标位置±0.3米范围内时认为任务完成并等待30秒后降落 - Eigen::Vector3d uav_pos = {uav_state.position[0], uav_state.position[1], uav_state.position[2]}; - float distance = (uav_pos - target_pos).norm(); - if(distance <= 0.3) - { - cout << GREEN << " UAV arrived at target position and landed after 30 seconds" << TAIL << endl; - sleep(30); - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //打印降落相关信息 - cout << GREEN << " UAV Land" << TAIL << endl; - cout << GREEN << " [ENU XYZ position control] tutorial_demo completed" << TAIL << endl; - //任务结束,关闭该节点 - ros::shutdown(); - } - else - { - //打印信息 - cout << GREEN << " [ " << distance << " ] m to the target point " << TAIL << endl; - } - } - } - else - { - //在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if(cmd_pub_flag) - { - cout << RED << " Unknown error! [ENU XYZ position control] tutorial_demo aborted" << TAIL << endl; - } - //命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else - { - cout << YELLOW << " Wait for UAV to enter [COMMAND_CONTROL] MODE " << TAIL << endl; - } - } - r.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/global_pos_control/launch/global_pos_control.launch b/src/无人机端代码/Modules/tutorial_demo/basic/global_pos_control/launch/global_pos_control.launch deleted file mode 100644 index 58e3af60..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/global_pos_control/launch/global_pos_control.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/global_pos_control/src/global_pos_control.cpp b/src/无人机端代码/Modules/tutorial_demo/basic/global_pos_control/src/global_pos_control.cpp deleted file mode 100644 index 7c97ca8f..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/global_pos_control/src/global_pos_control.cpp +++ /dev/null @@ -1,164 +0,0 @@ -/****************************************************************************** -*例程简介: 讲解如何调用uav_control的接口实现无人机的经纬度以及相对高度控制 -* -*效果说明: 无人机起飞移动到目标经纬度以及相对高度位置点,悬停30秒后降落 -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ - -#include -#include -#include -#include -#include -#include "printf_utils.h" - -using namespace std; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; - -//无人机状态回调函数 -void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; -} - -//无人机控制状态回调函数 -void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -//主函数 -int main(int argc, char** argv) -{ - //ROS初始化,设定节点名 - ros::init(argc , argv, "global_pos_control"); - //创建句柄 - ros::NodeHandle n; - - //声明起飞高度,无人机id变量 - float takeoff_height; - int uav_id; - //获取起飞高度参数 - ros::param::get("/uav_control_main_1/control/Takeoff_height", takeoff_height); - ros::param::get("~uav_id", uav_id); - - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 10); - //创建无人机状态命令订阅者 - ros::Subscriber uav_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", 10, uav_state_cb); - //创建无人机控制状态命令订阅者 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", 10, uav_control_state_cb); - //循环频率设置为1HZ - ros::Rate r(1); - //创建命令发布标志位,命令发布则为true;初始化为false - bool cmd_pub_flag = false; - - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为2位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - - //打印demo相关信息 - cout << GREEN << " [Global position control] tutorial_demo start " << TAIL << endl; - sleep(1); - cout << GREEN << " Level: [Basic] " << TAIL << endl; - sleep(1); - cout << GREEN << " Please use the RC SWA to armed, and the SWB to switch the drone to [COMMAND_CONTROL] mode " << TAIL << endl; - - while(ros::ok()) - { - //调用一次回调函数 - ros::spinOnce(); - //检测无人机是否处于[COMMAND_CONTROL]模式 - if(uav_control_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - //检测控制命令是否发布,没有发布则进行命令的发布 - if(!cmd_pub_flag) - { - //检测无人机是否到达预设起飞高度 - if(fabs(uav_state.position[2] - takeoff_height) >= 0.3) - { - continue; - } - cout << GREEN << " UAV takeoff success" << TAIL << endl; - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "WGS84"; - //Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - //Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::LAT_LON_ALT; - //在无人机当前经纬度飞到5米高度 - uav_command.latitude = uav_state.latitude; - uav_command.longitude = uav_state.longitude; - uav_command.altitude = 5.0; - uav_command.yaw_ref = 0.0; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //命令发布标志位置为true - cmd_pub_flag = true; - - //打印无人机控制命令发布信息 - cout << GREEN << " [LAT_LON_ALT] command publish " << TAIL << endl; - } - else - { - //当无人机距离高度目标值±0.3米范围内时认为任务完成并等待30秒后降落 - if(fabs(uav_state.position[2] - 5.0) <= 0.3) - { - cout << GREEN << " UAV arrived at target global position and landed after 30 seconds" << TAIL << endl; - sleep(30); - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //打印降落相关信息 - cout << GREEN << " UAV Land" << TAIL << endl; - cout << GREEN << " [Global position control] tutorial_demo completed" << TAIL << endl; - //任务结束,关闭该节点 - ros::shutdown(); - } - else - { - //打印当前无人机高度信息 - cout << GREEN << " UAV height: " << uav_state.position[2] << " [m]" << TAIL << endl; - } - } - } - else - { - //在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if(cmd_pub_flag) - { - cout << RED << " Unknown error! [Global position control] tutorial_demo aborted" << TAIL << endl; - } - //命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else - { - cout << YELLOW << " Wait for UAV to enter [COMMAND_CONTROL] MODE " << TAIL << endl; - } - } - r.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/launch/takeoff_land.launch b/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/launch/takeoff_land.launch deleted file mode 100644 index 360aff60..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/launch/takeoff_land.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/launch/takeoff_land_liu.launch b/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/launch/takeoff_land_liu.launch deleted file mode 100644 index 1fdd8192..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/launch/takeoff_land_liu.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/scripts/takeoff_land.py b/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/scripts/takeoff_land.py deleted file mode 100644 index 25a99817..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/scripts/takeoff_land.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# 例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的位置控制 -# 效果说明: 无人机起飞后悬停30秒,然后降落 -# 备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -from math import fabs -from turtle import position -import ros -import rospy -from prometheus_msgs.msg import UAVCommand, UAVControlState, UAVState - -# 创建无人机相关数据变量 -uav_control_state_sv = UAVControlState() -uav_command_pv = UAVCommand() -uav_state_sv = UAVState() - -# 无人机状态回调函数 -def uavStateCb(msg): - global uav_state_sv - uav_state_sv = msg - -# 无人机控制状态回调函数 -def uavControlStateCb(msg): - global uav_control_state_sv - uav_control_state_sv = msg - -def main(): - # ROS初始化,设定节点名 - rospy.init_node('takeoff_land_py',anonymous=True) - # 创建命令发布标志位,命令发布则为true;初始化为false - cmd_pub_flag = False - # 创建无人机控制命令发布者 - UavCommandPb = rospy.Publisher("/uav1/prometheus/command", UAVCommand, queue_size =10) - # 创建无人机控制状态命令订阅者 - rospy.Subscriber("/uav1/prometheus/control_state", UAVControlState, uavControlStateCb) - # 创建无人机状态命令订阅者 - rospy.Subscriber("/uav1/prometheus/state", UAVState, uavStateCb) - # 循环频率设置为10HZ - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - # 检测无人机是否处于[COMMAND_CONTROL]模式 - if uav_control_state_sv.control_state == UAVControlState.COMMAND_CONTROL: - # 检测控制命令是否发布,没有发布则进行命令的发布 - if not cmd_pub_flag: - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = 'ENU' - # Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度 - uav_command_pv.Agent_CMD = 1 - # 发布的命令ID,每发一次,该ID加1 - uav_command_pv.Command_ID = 1 - # 发布起飞命令 - UavCommandPb.publish(uav_command_pv) - rate.sleep() - # 命令发布标志位置为true - cmd_pub_flag = True - # 打印无人机起飞相关信息 - rospy.loginfo("Takeoff_height: %d", rospy.get_param('/uav_control_main_1/control/Takeoff_height')) - else: - # 当无人机距离高度目标值±0.1米范围内时认为起飞完成并等待30秒后降落 - if fabs(uav_state_sv.position[2] - rospy.get_param('/uav_control_main_1/control/Takeoff_height')) <= 0.1: - print(" UAV takeoff successfully and landed after 30 seconds") - rospy.sleep(30) - # 时间戳 - uav_command_pv.header.stamp = rospy.Time.now() - # 坐标系 - uav_command_pv.header.frame_id = "ENU" - # Land降落,从当前位置降落至地面并自动上锁 - uav_command_pv.Agent_CMD = 3 - # 发布的命令ID加1 - uav_command_pv.Command_ID += 1 - # 发布降落命令 - UavCommandPb.publish(uav_command_pv) - # 打印降落相关信息 - rospy.loginfo("UAV Land") - rospy.loginfo("[takeoff & land tutorial_demo completed]") - # 任务结束,关闭该节点 - rospy.signal_shutdown("shutdown time") - else: - # 打印当前无人机高度信息 - rospy.loginfo("UAV height : %f [m]", uav_state_sv.position[2]) - rospy.sleep(1) - else: - # 在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if cmd_pub_flag: - rospy.logfatal(" Unknown error! [takeoff & land] tutorial_demo aborted") - # 命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else: - rospy.logwarn(" Wait for UAV to enter [COMMAND_CONTROL] MODE ") - rospy.sleep(2) - rate.sleep() - rospy.spin() - -if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/CMakeLists.txt b/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/CMakeLists.txt deleted file mode 100644 index 66dd650a..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land.cpp b/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land.cpp deleted file mode 100644 index 62471232..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land.cpp +++ /dev/null @@ -1,151 +0,0 @@ -/****************************************************************************** -*例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的位置控制 -* -*效果说明: 无人机起飞后悬停30秒,然后降落 -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ -#include -#include -#include -#include -#include -#include "printf_utils.h" - -using namespace std; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; - -//无人机状态回调函数 -void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; -} - -//无人机控制状态回调函数 -void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -//主函数 -int main(int argc, char** argv) -{ - //ROS初始化,设定节点名 - ros::init(argc , argv, "takeoff_land"); - //创建句柄 - ros::NodeHandle n; - - //声明起飞高度,无人机id变量 - float takeoff_height; - int uav_id; - //获取起飞高度参数 - ros::param::get("/uav_control_main_1/control/Takeoff_height", takeoff_height); - ros::param::get("~uav_id", uav_id); - - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 10); - //创建无人机状态命令订阅者 - ros::Subscriber uav_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", 10, uav_state_cb); - //创建无人机控制状态命令订阅者 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", 10, uav_control_state_cb); - //循环频率设置为1HZ - ros::Rate r(1); - //创建命令发布标志位,命令发布则为true;初始化为false - bool cmd_pub_flag = false; - - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为2位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - - //打印demo相关信息 - cout << GREEN << " [takeoff & land] tutorial_demo start " << TAIL << endl; - sleep(1); - cout << GREEN << " Level: [Basic] " << TAIL << endl; - sleep(1); - cout << GREEN << " Please use the RC SWA to armed, and the SWB to switch the drone to [COMMAND_CONTROL] mode " << TAIL << endl; - - while(ros::ok()) - { - //调用一次回调函数 - ros::spinOnce(); - //检测无人机是否处于[COMMAND_CONTROL]模式 - if(uav_control_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - //检测控制命令是否发布,没有发布则进行命令的发布 - if(!cmd_pub_flag) - { - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - //发布的命令ID,每发一次,该ID加1 - uav_command.Command_ID = 1; - //发布起飞命令 - uav_command_pub.publish(uav_command); - //命令发布标志位置为true - cmd_pub_flag = true; - //打印无人机起飞相关信息 - cout << GREEN << " [Init_Pos_Hover] command publish " << TAIL << endl; - cout << GREEN << " UAV takeoff " << TAIL << endl; - cout << GREEN << " Takeoff_height: " << takeoff_height << " [m]" << TAIL << endl; - } - else - { - //当无人机距离高度目标值±0.3米范围内时认为起飞完成并等待30秒后降落 - if(fabs(uav_state.position[2] - takeoff_height) <= 0.3) - { - cout << GREEN << " UAV takeoff successfully and landed after 30 seconds" << TAIL << endl; - sleep(30); - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //打印降落相关信息 - cout << GREEN << " UAV Land" << TAIL << endl; - cout << GREEN << " [takeoff & land] tutorial_demo completed" << TAIL << endl; - //任务结束,关闭该节点 - ros::shutdown(); - } - else - { - //打印当前无人机高度信息 - cout << GREEN << " UAV height: " << uav_state.position[2] << " [m]" << TAIL << endl; - } - } - } - else - { - //在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if(cmd_pub_flag) - { - cout << RED << " Unknown error! [takeoff & land] tutorial_demo aborted" << TAIL << endl; - } - //命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else - { - cout << YELLOW << " Wait for UAV to enter [COMMAND_CONTROL] MODE " << TAIL << endl; - } - } - r.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land_liu.cpp b/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land_liu.cpp deleted file mode 100644 index 40872786..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land_liu.cpp +++ /dev/null @@ -1,255 +0,0 @@ -/****************************************************************************** -*例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的XY速度Z位置控制以及 -* 轨迹控制 -* -*效果说明: 无人机起飞后开始按照圆的轨迹飞行,飞行结束后悬停30秒,然后降落 -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ - - -#include -#include -#include -#include -#include -#include -#include -#include "printf_utils.h" - -using namespace std; - -const float PI = 3.1415926; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; -//创建圆形跟踪的相关变量 -//整个圆形的飞行时间 -float circular_time; -//每次控制数据更新时的弧度增量 -float angle_increment; -//无人机的合速度也就是圆的线速度 -float velocity; -//无人机的控制频率 -float control_rate; -//圆的半径 -float radius; - -//通过设定整个圆的飞行时间,控制频率,圆的半径来获取相关参数 -void get_circular_property(float time, int rate, float radius) -{ - //计算角速度(rad/s) - float w = 2*PI/time; - //计算线速度(m/s) - velocity = radius * w; - //计算控制数据发布的弧度增量 - angle_increment = w/rate; -} - - - -//无人机状态回调函数 -void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; -} - -//无人机控制状态回调函数 -void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -//主函数 -int main(int argc, char** argv) -{ - //ROS初始化,设定节点名 - ros::init(argc , argv, "body_xyz_pos_control"); - //创建句柄 - ros::NodeHandle n; - //获取无人机id - int uav_id; - ros::param::get("~uav_id", uav_id); - - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 10); - //创建无人机状态命令订阅者 - ros::Subscriber uav_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", 10, uav_state_cb); - //创建无人机控制状态命令订阅者 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", 10, uav_control_state_cb); - //循环频率设置为1HZ - ros::Rate r(1); - //创建命令发布标志位,命令发布则为true;初始化为false - bool cmd_pub_flag = false; - - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为2位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - - //打印demo相关信息 - cout << GREEN << " [circular trajectory control] tutorial_demo start " << TAIL << endl; - sleep(1); - cout << GREEN << " Level: [Basic] " << TAIL << endl; - sleep(1); - cout << GREEN << " Please use the RC SWA to armed, and the SWB to switch the drone to [COMMAND_CONTROL] mode " << TAIL << endl; - - //设定飞行时间为40S,控制频率为20HZ,半径为2米 - circular_time = 40; - control_rate = 20; - radius = 2; - - get_circular_property(circular_time, control_rate, radius); - - bool circular_success = false; - int count = 0; - ros::Rate rate(control_rate); - - Eigen::Vector3d target_pos = {radius, 0.0, 2.0}; - Eigen::Vector3d leader_pos = {0.0, 0.0, 0.0}; - - std::cout <> leader_pos[0]; - std::cout<> leader_pos[1]; - std::cout <> leader_pos[2]; - while(ros::ok()) - { - //调用一次回调函数 - ros::spinOnce(); - //检测无人机是否处于[COMMAND_CONTROL]模式 - - if(uav_control_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - //检测控制命令是否发布,没有发布则进行命令的发布 - if(!cmd_pub_flag) - { - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - //Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - //无人机将会以当前位置移动 - uav_command.position_ref[0] = target_pos[0] + leader_pos[0]; - uav_command.position_ref[1] = target_pos[1] + leader_pos[1]; - uav_command.position_ref[2] = target_pos[2] + leader_pos[2]; - uav_command.yaw_ref = 0.0; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //命令发布标志位置为true - cmd_pub_flag = true; - - //打印无人机控制命令发布信息 - cout << GREEN << " Go to initial point " << TAIL << endl; - - //检测无人机是否到达初始点 - bool initial_point_success = false; - while(!initial_point_success) - { - ros::spinOnce(); - //当无人机距离目标位置±0.1米范围内时认为到达初始点 - Eigen::Vector3d uav_pos = {uav_state.position[0], uav_state.position[1], uav_state.position[2]}; - float distance = (uav_pos - target_pos - leader_pos).norm(); - if(distance < 0.1) - { - cout << GREEN << " UAV arrived at initial point " << TAIL << endl; - initial_point_success = true; - } - r.sleep(); - } - } - else - { - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - //Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS; - //无人机按照圆形轨迹飞行 - uav_command.velocity_ref[0] = -velocity*sin(count*angle_increment); - uav_command.velocity_ref[1] = velocity*cos(count*angle_increment); - uav_command.velocity_ref[2] = 0; - uav_command.position_ref[2] = target_pos[2]; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //计数器 - if(count == control_rate*circular_time) - { - circular_success = true; - } - count++; - cout << GREEN << " Vx: " << uav_command.velocity_ref[0] << TAIL << endl; - cout << GREEN << " Vy: " << uav_command.velocity_ref[1] << TAIL << endl; - - if(circular_success) - { - cout << GREEN << " UAV circular trajectory completed and landed after 30 seconds" << TAIL << endl; - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - sleep(30); - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //打印降落相关信息 - cout << GREEN << " UAV Land" << TAIL << endl; - cout << GREEN << " [circular trajectory control] tutorial_demo completed" << TAIL << endl; - //任务结束,关闭该节点 - ros::shutdown(); - } - } - } - else - { - //在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if(cmd_pub_flag) - { - cout << RED << " Unknown error! [ENU XYZ position control] tutorial_demo aborted" << TAIL << endl; - r.sleep(); - continue; - } - //命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else - { - cout << YELLOW << " Wait for UAV to enter [COMMAND_CONTROL] MODE " << TAIL << endl; - r.sleep(); - continue; - } - } - rate.sleep(); - } - return 0; -} diff --git a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land_no_rc.cpp b/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land_no_rc.cpp deleted file mode 100644 index bdc0d334..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/basic/takeoff_land/src/takeoff_land_no_rc.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/****************************************************************************** -*例程简介: 讲解如何调用uav_control的接口实现无人机ENU坐标系下的位置控制 -* -*效果说明: 无人机起飞后悬停30秒,然后降落 -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ -#include -#include -#include -#include -#include - -#include -#include "printf_utils.h" - -using namespace std; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; - -//无人机状态回调函数 -void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; -} - -//无人机控制状态回调函数 -void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -//主函数 -int main(int argc, char** argv) -{ - //ROS初始化,设定节点名 - ros::init(argc , argv, "takeoff_land_no_rc"); - //创建句柄 - ros::NodeHandle n; - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav1/prometheus/command", 10); - //创建无人机控制命令发布者 - ros::Publisher uav_setup_pub = n.advertise("/uav1/prometheus/setup", 10); - //创建无人机状态命令订阅者 - ros::Subscriber uav_state_sub = n.subscribe("/uav1/prometheus/state", 10, uav_state_cb); - //创建无人机控制状态命令订阅者 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav1/prometheus/control_state", 10, uav_control_state_cb); - //循环频率设置为1HZ - ros::Rate r(1); - //创建命令发布标志位,命令发布则为true;初始化为false - bool cmd_pub_flag = false; - - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为2位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - - //声明起飞高度变量 - float takeoff_height; - - //获取起飞高度参数 - ros::param::get("/uav_control_main_1/control/Takeoff_height", takeoff_height); - - //打印demo相关信息 - cout << GREEN << " [takeoff & land] tutorial_demo start " << TAIL << endl; - sleep(1); - cout << GREEN << " Level: [Basic] " << TAIL << endl; - sleep(1); - cout << GREEN << " Please use the RC SWA to armed, and the SWB to switch the drone to [COMMAND_CONTROL] mode " << TAIL << endl; - - while(ros::ok()) - { - //调用一次回调函数 - ros::spinOnce(); - - // 解锁+切换到COMMAND_CONTROL模式 - if(uav_control_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - prometheus_msgs::UAVSetup uav_setup; - if(!uav_state.armed) - { - uav_setup.cmd = prometheus_msgs::UAVSetup::ARMING; - uav_setup.arming = true; - }else - { - uav_setup.cmd = prometheus_msgs::UAVSetup::SET_CONTROL_MODE; - uav_setup.control_state = "COMMAND_CONTROL"; - } - uav_setup_pub.publish(uav_setup); - } - - //检测无人机是否处于[COMMAND_CONTROL]模式 - if(uav_control_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - //检测控制命令是否发布,没有发布则进行命令的发布 - if(!cmd_pub_flag) - { - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Init_Pos_Hover初始位置悬停,可在uav_control_indoor.yaml或uav_control_outdoor.yaml文件设置无人机悬停高度 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - //发布的命令ID,每发一次,该ID加1 - uav_command.Command_ID = 1; - //发布起飞命令 - uav_command_pub.publish(uav_command); - //命令发布标志位置为true - cmd_pub_flag = true; - //打印无人机起飞相关信息 - cout << GREEN << " [Init_Pos_Hover] command publish " << TAIL << endl; - cout << GREEN << " UAV takeoff " << TAIL << endl; - cout << GREEN << " Takeoff_height: " << takeoff_height << " [m]" << TAIL << endl; - } - else - { - //当无人机距离高度目标值±0.3米范围内时认为起飞完成并等待30秒后降落 - if(fabs(uav_state.position[2] - takeoff_height) <= 0.3) - { - cout << GREEN << " UAV takeoff successfully and landed after 30 seconds" << TAIL << endl; - sleep(30); - //时间戳 - uav_command.header.stamp = ros::Time::now(); - //坐标系 - uav_command.header.frame_id = "ENU"; - //Land降落,从当前位置降落至地面并自动上锁 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - //发布的命令ID加1 - uav_command.Command_ID += 1; - //发布降落命令 - uav_command_pub.publish(uav_command); - //打印降落相关信息 - cout << GREEN << " UAV Land" << TAIL << endl; - cout << GREEN << " [takeoff & land] tutorial_demo completed" << TAIL << endl; - //任务结束,关闭该节点 - ros::shutdown(); - } - else - { - //打印当前无人机高度信息 - cout << GREEN << " UAV height: " << uav_state.position[2] << " [m]" << TAIL << endl; - } - } - } - else - { - //在控制命令发布后,但无人机未结束任务的情况下,此时无人机未处于[COMMAND_CONTROL]控制状态,认为无人机出现意外情况,任务中止 - if(cmd_pub_flag) - { - cout << RED << " Unknown error! [takeoff & land] tutorial_demo aborted" << TAIL << endl; - } - //命令未发布,等待无人机进入[COMMAND_CONTROL]状态 - else - { - cout << YELLOW << " Wait for UAV to enter [COMMAND_CONTROL] MODE " << TAIL << endl; - } - } - r.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/tutorial_demo/package.xml b/src/无人机端代码/Modules/tutorial_demo/package.xml deleted file mode 100644 index 4f38609c..00000000 --- a/src/无人机端代码/Modules/tutorial_demo/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - prometheus_demo - 0.0.0 - The prometheus_demo package - - Yuhua Qi - - TODO - - message_generation - message_runtime - catkin - std_msgs - std_msgs - std_msgs - - - - -