diff --git a/README.md b/README.md index a4b25cb..32924cc 100644 --- a/README.md +++ b/README.md @@ -10,5 +10,6 @@ RMUA2021战队技术答辩视频链接：[RoboMaster 2021高校人工智能挑 ## 文件结构 |文件夹名称|功能| |:--|:--| -|robot_sentry|哨岗识别算法| - +|robot_sentry|哨岗识别模块| +|robot_decision|决策模块| +|robot_detection|装甲识别模块| diff --git a/robot_detection/CMakeLists.txt b/robot_detection/CMakeLists.txt new file mode 100755 index 0000000..36732cf --- /dev/null +++ b/robot_detection/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robot_detection) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) +set(CMAKE_CXX_STANDARD 14) +## 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) +find_package(OpenCV 3 REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + roscpp + robot_msgs + roslib + image_transport + cv_bridge +) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES robot_camera +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +include_directories( + include + armor_detect + gimbal_control + kalman_filter + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRECTORIES} +) + +add_subdirectory(kalman_filter) + +add_executable(${PROJECT_NAME} + detection_node.cpp + armor_detect/armor_detection.cpp + gimbal_control/gimbal_control.cpp +) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} + kalman_filter + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) diff --git a/robot_detection/README.md b/robot_detection/README.md new file mode 100755 index 0000000..0daef6f --- /dev/null +++ b/robot_detection/README.md @@ -0,0 +1,197 @@ +# Critical HIT RMUA 辅瞄模块 + + +## 1、硬件及依赖环境 + +#### 车辆图片 + +![车辆图片](picture/车子图片.png) + + + +#### 硬件 + +车载相机用于检测敌方装甲模块，需要快速、稳定的识别方案。因此用于识别装甲需要高帧率、功能稳定的相机。另外因为装甲检测算法是通过对装甲模块两边的灯条的几何关系进行检测的，为了实现更好的检测效果， 我们需要能够调节相机的曝光度和亮度等。基于上述需求，我们选择了迈德威视的MV-SUA133GC-T 型号的 USB3.0 彩色工业相机，体积小，支持 PC Linux 系统，可支持输出 24bit RGB 图像数据。 + + + +#### 依赖环境 + + + +1.*ros（机器人操作系统）* + +整个工程都是基于ROS的软件框架，实现模块单独的测试，模块之间的通信、配合。 + + + +2.*OpenCV4* + +OpenCV库主要用来进行图像的形态学处理以及几何特征的处理。 + + + +3.*Eigen*3 + +运用Eigen库主要进行基本线性代数算法比如卡尔曼滤波中矩阵运算等。 + + + +4.*robot_msgs* + +自定义消息包，实现各模块之间通信。 + +## 2、软件介绍和代码说明 + +​自瞄模块的功能是获取敌人位姿信息，实现自动控制云台，跟踪击打装甲板，模块主要分为三个部分： +1. 装甲识别； +2. 云台转动角计算及卡尔曼滤波预测； +3. 辅助瞄准。 + +其中，装甲板检测完全基于传统的OpenCV算法实现，采取传统的灯条检测方案，主要步骤分为筛选灯条、灯条组合装甲、筛选装甲三个步骤。最终自瞄第一人称效果如下： + +

+ + +### 1.文件结构 + +```bash +robot_detection +├── CMakeLists.txt +├── camera_param #相机内参 +├── armor_detect # 装甲识别 +│ ├── armor_detection.cpp +│ └── armor_detection.h +├── gimbal_control # 云台转动角度计算，内含抛物线模型 +│ ├── gimbal_control.cpp +│ └── gimbal_controls.h +├── kalman_filter # 卡尔曼滤波库 +│ ├── CMakeLists.txt +│ ├── kalman_filter.cpp +│ └── kalman_filter.h +├── launch # launch文件 +├── detection_node.cpp # 主函数 +├── detection_node.hpp +└── package.xml +``` + + + +### 2. 代码结构 + +辅瞄模块主要包括装甲检测和辅助瞄准两部分。 + +#### 装甲检测 + +装甲检测部分使用传统CV算法，我们只检测灯条，没有识别数字，因此相机曝光时间调的很短，大体思路与[官方开源代码](https://github.com/RoboMaster/RoboRTS)一致，先二值化然后提取轮廓，然后根据几何关系对轮廓进行拟合筛选，最后组合成装甲，根据装甲的角点进行PnP解算，得到装甲板的坐标。 + +![装甲检测](picture/装甲板识别流程.png) + +- 装甲板识别过程及效果如图：左上是通道相减阈值分割结果，左下是亮度阈值分割结果，右下是初步装甲板，右上是结果装甲板。 + +![装甲板识别效果](picture/装甲板识别效果.png) + +

+ + + + +这一部分代码在`armor_detect`文件夹下。 + +#### 自瞄优先级 + +我们在每一轮装甲检测之后都会保存上一轮目标装甲的中心的在图片中的坐标(x,y)。然后，先是根据本轮候选装甲板中心的坐标，遍历比对，找到距离上一轮目标装甲板最近的装甲板A，判断为同一块装甲板，同时找到距离自身最近的一块装甲板B。之后进行下一轮比对，如果A和B相距本车的距离差不多，则瞄准A；如果B的距离明显小于A的距离，则瞄准B。 + +这一部分代码在`armor_detect`文件夹下。 + +#### 弹道模型和云台角度解算 + +- 弹道模型使用的是官方开源代码中的抛物线弹道模型，调试的时候有两个参数：发射速度和空气阻力系数。模型在`gimbal_control.cpp`中有所体现。具体弹道模型可参考文件夹中的`弹道模型.pdf`文件。 + + 这一部分代码在`gimbal_control`文件夹下。 + +![弹道模型图片](picture/弹道模型.png) + +- 云台角度解算使用图像中装甲的四个顶点，结合装甲的实际尺寸进行 PnP 解算获取到装甲相对于云台转轴的坐标位置，根据几何关系计算出yaw 轴和 pitch 轴的角度。 + +- 卡尔曼滤波实现云台角度跟随，视觉识别的结果通常带有噪声和延迟，我们希望能够从具有噪声的结果中估计出敌方机器人的速度，此时便能用上卡尔曼滤波来实现预测估计，达到自动跟随瞄准的目的。卡尔曼滤波器的设计如下图： + + ![卡尔曼滤波器](picture/卡尔曼滤波器.png) + +​ + +- 卡尔曼滤波的效果图如下：红色曲线是上层卡尔曼滤波后发布的角度，蓝色曲线是云台反馈的角度，基本能够实现很好地预测跟随。 + +

+ + + + +#### 辅助瞄准 + +我们的辅助瞄准有两种模式，一种是根据装甲识别结果进行瞄准，一种是根据哨岗识别结果进行瞄准，其中装甲识别瞄准的优先级高，两者之间的切换逻辑简要说明就是识别到装甲板之后，先判断是否在视野里找到敌人装甲板，如果没找到就用哨岗识别结果进行全地图瞄准；如果在视野中找到了敌人装甲板，判断距离是否少于6m，是的话就根据装甲板识别结果进行瞄准，否的话就根据哨岗识别结果进行瞄准；如果前一帧发现敌人，这一帧敌人小消失，为防止灯条的击打闪烁则等待几帧。 + +![辅助瞄准](picture/辅助瞄准.png) + +具体逻辑实现可见`detection_node.cpp`中。 + + + +### 3.参数 + +- 相机内参 + +用于pnp解算目标装甲板的位姿，这部分在`camera_param`文件夹下。 + +- 装甲板识别参数 + +主要是对图片进行形态学处理的阈值以及装甲板筛选条件的阈值，主要需要调整参数有 + +| 参数名称 | 参数含义 | +| -------------------------------------------------- | ------------------------------------- | +| enemy_color | 敌人颜色，需要上场前设定 | +| red_brightness_threshold/blue_brightness_threshold | 针对红色/蓝色的亮度分割阈值 | +| red_threshold/blue_threshold | 通道分离后红色/蓝色图像的二值分割阈值 | + +这部分参数直接在`robot_detection.launch`文件中更改。 + +- 弹道模型参数 + +主要需要调整参数是发射速度`bullet_init_v`和空气阻力系数`bullet_init_k`。 + +这部分参数主要在`robot_detection.launch`文件中更改。 + + + +### 4.各模块运行速度 + +- 测试所用电脑配置：CPU Intel® Core™ i7-1165G7 ,GPU NVIDIA GeForce RTX 2060 + +- 整套代码耗时：4ms左右 + +- 主要模块耗时： + + ​ 1.获取灯条:1.9ms左右 + + ​ 2.配对灯条获取装甲板:0.005ms左右 + + ​ 3.筛选装甲板及位姿解算:0.25ms左右 + + ​ 4.计算云台转角:0.02ms以下 + + ![耗时](picture/耗时.png) + + +## 3、 代码编译、运行说明 + +测试需要配合播放rosbag或者自行发布图像话题`/mvsua_cam/image_raw1`。 + +单独运行测试：进入工作空间，运行节点，播放rosbag + +```bash +source devel/setup.bash +catkin_make +roslaunch robot_detection robot_detection.launch +rosbag play test.bag +``` + diff --git a/robot_detection/armor_detect/armor_detection.cpp b/robot_detection/armor_detect/armor_detection.cpp new file mode 100755 index 0000000..4760335 --- /dev/null +++ b/robot_detection/armor_detect/armor_detection.cpp @@ -0,0 +1,354 @@ +#include "armor_detection.h" + +namespace robot_detection { + ArmorDetection::ArmorDetection() { + last_armor_center_ = cv::Point2f(0, 0); + armor_height = 60; + armor_width = 127.5; + SolveArmorCoordinate(armor_width, armor_height); + } + + void ArmorDetection::LoadParam(bool color, + bool enable_debug, + double light_min_angle, + double light_max_angle_diff, + double light_aspect_ratio, + double armor_max_angle, + double armor_max_aspect_ratio, + double armor_min_aspect_ratio, + int blue_threshold, + int red_threshold, + int red_brightness_threshold, + int blue_brightness_threshold, + cv::Mat intrinsic_matrix, cv::Mat distortion_coeffs) { + enemy_color_ = color; + enable_debug_ = enable_debug; + light_min_angle_ = light_min_angle; + light_max_angle_ = 180 - light_min_angle_; + light_max_angle_diff_ = light_max_angle_diff; + light_aspect_ratio_ = light_aspect_ratio; + armor_max_angle_ = armor_max_angle; + armor_max_aspect_ratio_ = armor_max_aspect_ratio; + armor_min_aspect_ratio_ = armor_min_aspect_ratio; + blue_threshold_ = blue_threshold; + red_threshold_ = red_threshold; + red_brightness_threshold_ = red_brightness_threshold; + blue_brightness_threshold_ = blue_brightness_threshold; + intrinsic_matrix_ = intrinsic_matrix; + distortion_coeffs_ = distortion_coeffs; + } + + void ArmorDetection::DetectArmor(cv::Mat &src_img_, bool &detected, cv::Point3f &target_3d, cv::Point3f &target_r) { + std::vector lights; + std::vector lights_angle; + std::vector armors; + + DetectLights(src_img_, lights, lights_angle); + PossibleArmors(lights, armors, lights_angle); + if (!armors.empty()) { + detected = true; + final_armor_ = SelectFinalArmor(armors, target_3d); + + last_armor_center_ = cv::Point2f(final_armor_.center.x, final_armor_.center.y); + + if (enable_debug_) { + cv::Point2f vertex[4]; + final_armor_.points(vertex); + for (int i = 0; i < 4; i++) + cv::line(src_img_, vertex[i], vertex[(i + 1) % 4], cv::Scalar(0, 255, 0), 2); + } + + // CalcControlInfo(src_img_, final_armor_, target_3d, target_r); + std::stringstream ss; + ss << (int)target_3d.x << "," << (int)target_3d.y << "," << (int)target_3d.z; + std::string res; + ss >> res; + cv::putText(src_img_, res, cv::Point(20,20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0,0,255), 1); + } else { + detected = false; + } + if(enable_debug_) { + cv::imshow("armors_after_filter", src_img_); + if(cv::waitKey(1) == 'q') + { + enable_debug_ = false; + cv::destroyAllWindows(); + } + } + } + + void ArmorDetection::DetectLights(const cv::Mat &src_img_, std::vector &lights, std::vector &lights_angle) { + std::vector bgr; + cv::Mat light, gray_img_, binary_brightness_img_; + cv::split(src_img_, bgr); + if (enemy_color_ == true) { + // red + cv::subtract(bgr[2], bgr[1], light); + } else if (enemy_color_ == false) { + // blue + cv::subtract(bgr[0], bgr[2], light); + } + cv::cvtColor(src_img_, gray_img_, CV_BGR2GRAY); + double thresh; + if (enemy_color_ == false) + { + thresh = blue_threshold_; + cv::threshold(gray_img_, binary_brightness_img_, blue_brightness_threshold_, 255, CV_THRESH_BINARY); + } + else + { + thresh = red_threshold_; + cv::threshold(gray_img_, binary_brightness_img_, red_brightness_threshold_, 255, CV_THRESH_BINARY); + } + cv::threshold(light, binary_color_img_, thresh, 255, CV_THRESH_BINARY); + cv::Mat close_element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(6, 6)); + cv::dilate(binary_color_img_, binary_color_img_, close_element); + cv::bitwise_and(binary_color_img_, binary_brightness_img_, binary_color_img_); + + if(enable_debug_) { + // cv::imshow("binary_color", binary_color_img_); + } + + std::vector> contours_brightness; + cv::findContours(binary_color_img_, contours_brightness, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE); + + lights.clear(); + + for (int i = 0; i < contours_brightness.size(); ++i) { + double armor_angle; + cv::RotatedRect single_light = cv::minAreaRect(contours_brightness[i]); + if(single_light.size.width < single_light.size.height) { + armor_angle = single_light.angle - 90; + } else { + armor_angle = single_light.angle; + } + armor_angle *= -1; + + auto light_aspect_ratio = std::max(single_light.size.width, single_light.size.height) / + std::min(single_light.size.width, single_light.size.height); + + if (armor_angle < light_max_angle_ && armor_angle > light_min_angle_ && light_aspect_ratio > light_aspect_ratio_) { + lights.push_back(single_light); + lights_angle.push_back(armor_angle); + } + } + + cv::imshow("gray_image", binary_color_img_); + } + + void ArmorDetection::PossibleArmors(const std::vector &lights, + std::vector &armors_possible, + std::vector &lights_angle) { + if(lights.size() < 2) return; + for(unsigned int i = 0; i < lights.size(); i++) { + for (unsigned int j = i + 1; j < lights.size(); j++) { + cv::RotatedRect light1 = lights[i]; + cv::RotatedRect light2 = lights[j]; + auto edge1 = std::minmax(light1.size.width, light1.size.height); + auto edge2 = std::minmax(light2.size.width, light2.size.height); + + if(std::fabs(light1.center.x - light2.center.x) < 1e-3) + continue; + + double armor_angle = std::atan((light1.center.y - light2.center.y) / + (light1.center.x - light2.center.x)) / CV_PI * 180.0; + double center_angle_for_rect; + if (armor_angle > 0) + center_angle_for_rect = armor_angle; + else + center_angle_for_rect = 180 + armor_angle; + if(std::abs(armor_angle) > armor_max_angle_) + { + continue; + } + auto angle_diff = std::abs(lights_angle[i] - lights_angle[j]); + if(angle_diff > light_max_angle_diff_) + { + continue; + } + auto lights_dis = std::sqrt((light1.center.x - light2.center.x) * (light1.center.x - light2.center.x) + + (light1.center.y - light2.center.y) * (light1.center.y - light2.center.y)); + double armor_height_max = std::max(edge1.second, edge2.second); + double armor_height_min = std::min(edge1.second, edge2.second); + if(std::fabs(armor_height_min) < 1e-3) + continue; + + if(lights_dis / armor_height_max > armor_max_aspect_ratio_) + { + continue; + } + if(lights_dis / armor_height_max < armor_min_aspect_ratio_) + { + continue; + } + if(armor_height_max / armor_height_min > 1.8) + { + continue; + } + cv::RotatedRect rect; + rect.angle = center_angle_for_rect; + if(rect.angle > 90) + { + rect.angle -= 180; + } + rect.center.x = (light1.center.x + light2.center.x) / 2; + rect.center.y = (light1.center.y + light2.center.y) / 2; + rect.size.width = lights_dis; + rect.size.height = armor_height_max; + armors_possible.emplace_back(rect); + } + } + if(armors_possible.size() < 2) return; + std::vector is_armor(armors_possible.size(), true); + for (int i = 0; i < armors_possible.size() - 1 && is_armor[i]; i++) { + for (int j = i + 1; j < armors_possible.size() && is_armor[j]; j++) { + double dx = armors_possible[i].center.x - armors_possible[j].center.x; + double dy = armors_possible[i].center.y - armors_possible[j].center.y; + double dis = std::sqrt(dx * dx + dy * dy); + // filter the armor reflected by the ground + if (std::abs(dy / dx) > 20 || std::fabs(dx) < 1e-3) { + if (dy > 0) //delete the lower armor + is_armor[i] = false; + else + is_armor[j] = false; + } + //if two armors have public area + auto edge1 = std::minmax(armors_possible[i].size.width, armors_possible[i].size.height); + auto edge2 = std::minmax(armors_possible[j].size.width, armors_possible[j].size.height); + if (dis <= (edge1.second + edge2.second)/2) { + double angle1 = fabs(armors_possible[i].angle); + double angle2 = fabs(armors_possible[j].angle); + if(fabs(angle1-angle2) > 5) + { + if (angle1 > angle2) + is_armor[i] = false; + else + is_armor[j] = false; + } + } + } + } + for (unsigned int i = 0; i < armors_possible.size(); i++) { + if(!is_armor[i]) { + armors_possible.erase(armors_possible.begin() + i); + is_armor.erase(is_armor.begin() + i); + //after erasing, i points to next element + i--; + } + } + } + + cv::RotatedRect ArmorDetection::SelectFinalArmor(std::vector &armors, cv::Point3f &target_3d_) { + cv::Point2f current_armor_center; + double center_dis, min_center_dis = 10000; + int last_index = 0, last_distance; + int return_index = 0; + int min_distance = 10000; + int nearest_index = 0; + cv::Point3f last_target, nearest_target; + + for (int i = 0; i < armors.size(); i++) { + cv::Mat rvec, tvec; + std::vector armor_points; + cv::Point2f armor_point[4]; + armors[i].points(armor_point); + double armor_angle; + if(armors[i].angle < 90) { + armor_angle = 90 - armors[i].angle; + } else { + armor_angle = 270 - armors[i].angle; + } + if(armor_angle >= 90) { + armor_points.push_back(armor_point[2]); + armor_points.push_back(armor_point[1]); + armor_points.push_back(armor_point[0]); + armor_points.push_back(armor_point[3]); + } else { + armor_points.push_back(armor_point[0]); + armor_points.push_back(armor_point[3]); + armor_points.push_back(armor_point[2]); + armor_points.push_back(armor_point[1]); + } + cv::solvePnP(armor_points_, + armor_points, + intrinsic_matrix_, + distortion_coeffs_, + rvec, + tvec); + cv::Point3f target_3d = cv::Point3f(tvec); + if(target_3d.z < min_distance) + { + min_distance = target_3d.z; + nearest_index = i; + nearest_target = target_3d; + } + + current_armor_center = cv::Point2f(armors[i].center.x, armors[i].center.y); + + center_dis = std::sqrt(std::pow(current_armor_center.x - last_armor_center_.x, 2) + + std::pow(current_armor_center.y - last_armor_center_.y, 2)); + + if (center_dis < min_center_dis) { + min_center_dis = center_dis; + last_index = i; + last_distance = target_3d.z; + last_target = target_3d; + } + } + if(std::abs(last_distance - min_distance) < 500) + { + return_index = last_index; + target_3d_ = last_target; + } + else + { + return_index = nearest_index; + target_3d_ = nearest_target; + } + return armors[return_index]; + } + + void ArmorDetection::CalcControlInfo(cv::Mat &src_img_, cv::RotatedRect &armor, cv::Point3f &target_3d, cv::Point3f &target_r) { + cv::Mat rvec; + cv::Mat tvec; + + std::vector armor_points; + cv::Point2f armor_point[4]; + armor.points(armor_point); + double armor_angle; + if(armor.angle < 90) { + armor_angle = 90 - armor.angle; + } else { + armor_angle = 270 - armor.angle; + } + if(armor_angle >= 90) { + armor_points.push_back(armor_point[2]); + armor_points.push_back(armor_point[1]); + armor_points.push_back(armor_point[0]); + armor_points.push_back(armor_point[3]); + } else { + armor_points.push_back(armor_point[0]); + armor_points.push_back(armor_point[3]); + armor_points.push_back(armor_point[2]); + armor_points.push_back(armor_point[1]); + } + + cv::solvePnP(armor_points_, + armor_points, + intrinsic_matrix_, + distortion_coeffs_, + rvec, + tvec); + target_3d = cv::Point3f(tvec); + target_r = cv::Point3f(rvec); + } + + void ArmorDetection::SolveArmorCoordinate(const double width, const double height) { + armor_points_.emplace_back(cv::Point3f(-width / 2, height / 2, 0.0)); + armor_points_.emplace_back(cv::Point3f(width / 2, height / 2, 0.0)); + armor_points_.emplace_back(cv::Point3f(width / 2, -height / 2, 0.0)); + armor_points_.emplace_back(cv::Point3f(-width / 2, -height / 2, 0.0)); + } + + ArmorDetection::~ArmorDetection() {} +} \ No newline at end of file diff --git a/robot_detection/armor_detect/armor_detection.h b/robot_detection/armor_detect/armor_detection.h new file mode 100755 index 0000000..96e3c78 --- /dev/null +++ b/robot_detection/armor_detect/armor_detection.h @@ -0,0 +1,79 @@ +#ifndef ARMOR_DETECTION_H +#define ARMOR_DETECTION_H + +#include + +namespace robot_detection { + class ArmorDetection { + public: + ArmorDetection(); + + void LoadParam(bool color, + bool enable_debug, + double light_min_angle, + double light_max_angle_diff, + double light_aspect_ratio, + double armor_max_angle, + double armor_max_aspect_ratio, + double armor_min_aspect_ratio, + int blue_threshold, + int red_threshold, + int red_brightness_threshold, + int blue_brightness_threshold, + cv::Mat intrinsic_matrix, cv::Mat distortion_coeffs); + + void DetectArmor(cv::Mat &src_img_, bool &detected, cv::Point3f &target_3d, cv::Point3f &target_r); + + void DetectLights(const cv::Mat &src_img_, std::vector &lights, std::vector &lights_angle); + + void PossibleArmors(const std::vector &lights, + std::vector &armors_possible, + std::vector &lights_angle); + + cv::RotatedRect SelectFinalArmor(std::vector &armors, cv::Point3f &target_3d_); + + void CalcControlInfo(cv::Mat &src_img_, cv::RotatedRect &armor, cv::Point3f &target_3d, cv::Point3f &target_r); + + void SolveArmorCoordinate(const double width, const double height); + + ~ArmorDetection(); + + private: + cv::Mat binary_color_img_; + + cv::Mat intrinsic_matrix_; + cv::Mat distortion_coeffs_; + + // Parameters come form .prototxt file + bool enable_debug_; + bool enemy_color_; + + //! armor size + double armor_width; + double armor_height; + + //! armor info + std::vector armor_points_; + cv::RotatedRect final_armor_; + + //! Filter lights + double light_min_angle_; + double light_max_angle_; + double light_max_angle_diff_; + double light_aspect_ratio_; + + //! Filter armor + double armor_max_angle_; + double armor_max_aspect_ratio_; + double armor_min_aspect_ratio_; + + cv::Point2f last_armor_center_; + + int red_threshold_; + int blue_threshold_; + int red_brightness_threshold_; + int blue_brightness_threshold_; + }; +} + +#endif \ No newline at end of file diff --git a/robot_detection/camera_param/camera_param_icra1.yaml b/robot_detection/camera_param/camera_param_icra1.yaml new file mode 100755 index 0000000..f5a88b6 --- /dev/null +++ b/robot_detection/camera_param/camera_param_icra1.yaml @@ -0,0 +1,15 @@ +%YAML:1.0 +cameraMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 1.0442461677304202e+003, 0., 6.2335358117180715e+002, 0., + 1.0442465557284168e+003, 1.1089855597048259e+002, 0., 0., 1. ] +distCoeffs: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ -1.0085718488402109e-001, 1.0963742883163741e-001, + -3.4455236650403156e-003, 6.5779086317056042e-004, + -2.7736841499705740e-002 ] +average_reprojection_err: 5.0971305569012959e-001 diff --git a/robot_detection/camera_param/camera_param_icra2.yaml b/robot_detection/camera_param/camera_param_icra2.yaml new file mode 100755 index 0000000..8e6049a --- /dev/null +++ b/robot_detection/camera_param/camera_param_icra2.yaml @@ -0,0 +1,15 @@ +%YAML:1.0 +cameraMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 1.0429903059265573e+003, 0., 6.7015942042688880e+002, 0., + 1.0430045819615750e+003, 6.6073482328914082e+001, 0., 0., 1. ] +distCoeffs: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ -9.4296144116818539e-002, 9.3151738521788513e-002, + -1.7994820026219758e-003, -1.7400089523901614e-003, + -7.6826321556826777e-003 ] +average_reprojection_err: 4.9128131912304807e-001 diff --git a/robot_detection/camera_param/camera_param_rmua1.yaml b/robot_detection/camera_param/camera_param_rmua1.yaml new file mode 100755 index 0000000..4c30168 --- /dev/null +++ b/robot_detection/camera_param/camera_param_rmua1.yaml @@ -0,0 +1,15 @@ +%YAML:1.0 +cameraMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 1039.28420644888, 0., 650.031229654822, 0., + 1040.43583935739, 91.4289178881700, 0., 0., 1. ] +distCoeffs: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ -0.101239733562301, 0.120279695097789, + -0.00232373422874182, 0.000566710572395489, + -0.0413959234793106 ] +average_reprojection_err: 5.1860811710357668e-001 diff --git a/robot_detection/camera_param/camera_param_rmua2.yaml b/robot_detection/camera_param/camera_param_rmua2.yaml new file mode 100755 index 0000000..b780837 --- /dev/null +++ b/robot_detection/camera_param/camera_param_rmua2.yaml @@ -0,0 +1,15 @@ +%YAML:1.0 +cameraMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 1039.41635326710, 0., 643.104393458956, 0., + 1040.36672424683, 85.8356342680557, 0., 0., 1. ] +distCoeffs: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ -0.0979095767581536, 0.102672780553020, + -0.00129119295476192, 0.000575939353198686, + -0.0176661394766689 ] +average_reprojection_err: 5.4189452372099223e-001 diff --git a/robot_detection/detection_node.cpp b/robot_detection/detection_node.cpp new file mode 100755 index 0000000..6f9857d --- /dev/null +++ b/robot_detection/detection_node.cpp @@ -0,0 +1,449 @@ +#include +#include +#include +#include "gimbal_control.h" +#include "armor_detection.h" +#include "robot_msgs/GimbalAngle.h" +#include "robot_msgs/ArmorDetection.h" +#include "robot_msgs/ZMQdata.h" +#include "geometry_msgs/PoseStamped.h" +#include +#include "kalman_filter.h" +#include +#include +#include "cv_bridge/cv_bridge.h" +void SignalHandler(int signal) { + ros::shutdown(); +} + +robot_detection::ArmorDetection armor_detector_; +robot_detection::GimbalContrl gimbal_control_; + +double last_pitch_; +double last_yaw_; +double delta_pitch, delta_yaw; +double pitch_raw_angle_; +double yaw_raw_angle_; +double yaw_ecd_angle_; +double gimbal_global_angle_; +int missed_cnt_ = 0; +int detected_cnt_ = 0; +int sentry_miss_cnt_ = 0; +double bullet_speed_; + +std::vector sentry_enemy_; +int current_sentry_id_ = 0; +bool change_target_flag_ = false; +cv::Point2d current_sentry_enemy_; +bool received_enemy_info_ = false; +std::string color_str; +cv::Point3d self_pose_; +bool received_self_pose_ = false; +int last_detected_cnt_ = 60; +int sentry_cnt_ = 200; + +ros::Subscriber gimbal_angle_sub_; +ros::Subscriber sentry_pose_sub_; +ros::Subscriber robot_pose_sub_; +image_transport::Subscriber image_sub_; +ros::Publisher enemy_info_pub_; +ros::Publisher armor_detection_pub_; + +bool converge_flag_ = false; +bool estimate_converge_flag_ = false; +bool switch_flag_ = false; +bool switch_process_flag_ = false; +bool detected_enemy_ = false; +bool last_detected_enemy_ = false; + +std::unique_ptr pitch_kf_; +std::unique_ptr yaw_kf_; +clock_t image_timestamp_; +clock_t image_last_timestamp_; +bool first_frame = true; + +void GimbalAngleCallback(const robot_msgs::GimbalAngle::ConstPtr &msg) +{ + static int bias_count_ = 0; + static double gimbal_global_angle_bias_; + pitch_raw_angle_ = msg->pitch_angle; + yaw_raw_angle_ = msg->yaw_angle; + yaw_ecd_angle_ = msg->yaw_ecd_angle; + bullet_speed_ = msg->bullet_speed; + if(bullet_speed_ > 1e-5) + gimbal_control_.set_velocity(bullet_speed_); + + if(fabs(yaw_ecd_angle_) < 2.0) + { + bias_count_++; + if(bias_count_ > 250) + { + // 校准云台偏置 + bias_count_ = 0; + gimbal_global_angle_bias_ = yaw_raw_angle_ - self_pose_.z * 180.0 / 3.14159265357 - yaw_ecd_angle_; + } + } + else + bias_count_ = 0; + double delta = yaw_raw_angle_ - gimbal_global_angle_bias_; + int round = delta / 360.0; + delta = delta - round * 360.0; + if(delta >= 180.0) gimbal_global_angle_ = -(360.0 - delta) * 3.14159265357 / 180.0; + else if(delta <= -180.0) gimbal_global_angle_ = (360.0 + delta) * 3.14159265357 / 180.0; + else gimbal_global_angle_ = delta * 3.14159265357 / 180.0; +} + +void EskfPoseCallback(const geometry_msgs::PoseStamped::ConstPtr msg) +{ + self_pose_.x = msg->pose.position.x; + self_pose_.y = msg->pose.position.y; + self_pose_.z = tf::getYaw(msg->pose.orientation); + received_self_pose_ = true; +} + +void SentryPoseCallback(const robot_msgs::ZMQdata::ConstPtr &msg) +{ + sentry_cnt_ = 0; + if(color_str == "blue") + { + sentry_enemy_[0].x = msg->robot_pose_x[0] / 100.0; + sentry_enemy_[0].y = msg->robot_pose_y[0] / 100.0; + sentry_enemy_[1].x = msg->robot_pose_x[1] / 100.0; + sentry_enemy_[1].y = msg->robot_pose_y[1] / 100.0; + } + else if(color_str == "red") + { + sentry_enemy_[0].x = msg->robot_pose_x[2] / 100.0; + sentry_enemy_[0].y = msg->robot_pose_y[2] / 100.0; + sentry_enemy_[1].x = msg->robot_pose_x[3] / 100.0; + sentry_enemy_[1].y = msg->robot_pose_y[3] / 100.0; + } + if((sentry_enemy_[0].x != 0. && sentry_enemy_[0].y != 0.) && + (sentry_enemy_[1].x != 0. && sentry_enemy_[1].y != 0.)) + { + received_enemy_info_ = true; + if(sentry_miss_cnt_ > 600) + { + current_sentry_id_ = 1 - current_sentry_id_; + sentry_miss_cnt_ = 0; + } + } + else if((sentry_enemy_[0].x != 0. && sentry_enemy_[0].y != 0.) || + (sentry_enemy_[1].x != 0. && sentry_enemy_[1].y != 0.)) + { + received_enemy_info_ = true; + if(sentry_enemy_[0].x != 0. && sentry_enemy_[0].y != 0.) + current_sentry_id_ = 0; + else + current_sentry_id_ = 1; + } + else + { + received_enemy_info_ = false; + } +} + +void ImageCallback(const sensor_msgs::ImageConstPtr &msg) +{ + // cv::imshow("img", image); + // int key = cv::waitKey(1); + // static int cnt = 0; + // if(key == 'q') + // { + // char str[10]; + // sprintf(str, "./%d.jpg", cnt); + // cv::imwrite(str, image); + // cnt++; + // } + // return; + + cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image.clone(); + + current_sentry_enemy_ = sentry_enemy_[current_sentry_id_]; + if(current_sentry_enemy_.x == 0. || current_sentry_enemy_.y == 0.) + received_enemy_info_ = false; + if(sentry_cnt_ > 100) + received_enemy_info_ = false; + else + sentry_cnt_++; + image_timestamp_ = std::clock(); + double delta_t = (double)(image_timestamp_ - image_last_timestamp_) / CLOCKS_PER_SEC; + + cv::Point3f target_3d_; + cv::Point3f target_r_; + double armor_dist = 0.0; + + if(!image.empty()) + { + armor_detector_.DetectArmor(image, detected_enemy_, target_3d_, target_r_); + } + double pitch, yaw; + robot_msgs::GimbalAngle gimbal_angle_; + robot_msgs::ArmorDetection armor_detection_; + + armor_dist = std::sqrt(target_3d_.x * target_3d_.x + target_3d_.y * target_3d_.y + target_3d_.z * target_3d_.z); + double predict_time = armor_dist / gimbal_control_.init_v_ / 1000.0; + + if (detected_enemy_) + { + sentry_miss_cnt_ = 0; + double rvec_norm = sqrt(target_r_.x*target_r_.x + target_r_.y*target_r_.y + target_r_.z*target_r_.z); + armor_detection_.detected_enemy = true; + gimbal_control_.Transform(target_3d_, delta_pitch, delta_yaw); + double distance = target_3d_.z; + gimbal_angle_.distance = target_3d_.z; + armor_detection_.distance = distance; + + pitch = delta_pitch; + yaw = delta_yaw + yaw_raw_angle_; + missed_cnt_ = 20; + + if(first_frame) { + Eigen::Vector3d pitch_reset, yaw_reset; + pitch_reset << 0, 0, 0; + yaw_reset << yaw_raw_angle_, 0, 0; + + pitch_kf_->Reset(pitch_reset); + yaw_kf_->Reset(yaw_reset); + first_frame = false; + return; + } + + detected_cnt_++; + + if(std::fabs(delta_yaw) < 2.0 && !converge_flag_) + converge_flag_ = true; + else if(std::fabs(delta_yaw) > 2.5 && converge_flag_) + converge_flag_ = false; + + if(std::abs(yaw - yaw_kf_->GetState().x()) < 1.0 && !estimate_converge_flag_) + estimate_converge_flag_ = true; + else if(std::abs(yaw - yaw_kf_->GetState().x()) > 1.5 && estimate_converge_flag_) + estimate_converge_flag_ = false; + + + if(std::fabs(yaw - last_yaw_) > 5.0 && last_detected_enemy_) + switch_flag_ = true; + else + switch_flag_ = false; + + if(switch_flag_) + switch_process_flag_ = true; + + if(switch_process_flag_) { + pitch_kf_->StateUpdate(delta_t, false); + pitch_kf_->MeasurementUpdate(pitch, false); + yaw_kf_->StateUpdate(delta_t, false); + yaw_kf_->MeasurementUpdate(yaw, false); + + if(converge_flag_) { + switch_process_flag_ = false; + } + } + else { + pitch_kf_->StateUpdate(delta_t, true); + pitch_kf_->MeasurementUpdate(pitch, true); + yaw_kf_->StateUpdate(delta_t, true); + yaw_kf_->MeasurementUpdate(yaw, true); + } + + gimbal_angle_.yaw_mode = 1; + last_detected_cnt_ = 0; + if(gimbal_angle_.distance > 6000 || gimbal_angle_.distance == 0){ + if(received_enemy_info_ && received_self_pose_) + { + double angle = std::atan2(current_sentry_enemy_.y - self_pose_.y, current_sentry_enemy_.x - self_pose_.x); + double angle_delta = angle - gimbal_global_angle_; + if(angle_delta > CV_PI) angle_delta -= 2 * CV_PI; + if(angle_delta < -CV_PI) angle_delta += 2 * CV_PI; + gimbal_angle_.yaw_mode = 1; + gimbal_angle_.yaw_angle = angle_delta * 0.1 * 180.0/CV_PI + yaw_raw_angle_; + } + else + { + gimbal_angle_.yaw_mode = 0; + } + } + gimbal_angle_.yaw_original = yaw; + + gimbal_angle_.yaw_angle = yaw_kf_->GetPredictState(predict_time).x(); + gimbal_angle_.pitch_angle = pitch_kf_->GetPredictState(predict_time).x(); + + + gimbal_angle_.yaw_original = yaw; + detected_cnt_ = 20; + } + else if (!detected_enemy_ && missed_cnt_ > 0) + { + sentry_miss_cnt_ = 0; + missed_cnt_--; + gimbal_angle_.yaw_mode = 1; + last_detected_cnt_ = 0; + if(gimbal_angle_.distance > 6000 || gimbal_angle_.distance == 0){ + if(received_enemy_info_ && received_self_pose_) + { + double angle = std::atan2(current_sentry_enemy_.y - self_pose_.y, current_sentry_enemy_.x - self_pose_.x); + double angle_delta = angle - gimbal_global_angle_; + if(angle_delta > CV_PI) angle_delta -= 2 * CV_PI; + if(angle_delta < -CV_PI) angle_delta += 2 * CV_PI; + gimbal_angle_.yaw_mode = 1; + gimbal_angle_.yaw_angle = angle_delta * 0.1 * 180.0/CV_PI + yaw_raw_angle_; + } + else + { + gimbal_angle_.yaw_mode = 0; + } + } + + pitch_kf_->PredictWithVelocity(delta_t); + yaw_kf_->PredictWithVelocity(delta_t); + gimbal_angle_.yaw_angle = yaw_kf_->GetPredictState(predict_time).x(); + gimbal_angle_.pitch_angle = pitch_kf_->GetPredictState(predict_time).x(); + gimbal_angle_.yaw_original = yaw_raw_angle_ + 10000.0; + } + else + { + sentry_miss_cnt_++; + converge_flag_ = false; + estimate_converge_flag_ = false; + switch_flag_ = false; + switch_process_flag_ = false; + + if(received_self_pose_ && received_enemy_info_) + { + if(last_detected_cnt_ < 60) last_detected_cnt_++; + double angle = std::atan2(current_sentry_enemy_.y - self_pose_.y, current_sentry_enemy_.x - self_pose_.x); + double angle_delta = angle - gimbal_global_angle_; + if(angle_delta > CV_PI) angle_delta -= 2 * CV_PI; + if(angle_delta < -CV_PI) angle_delta += 2 * CV_PI; + gimbal_angle_.yaw_mode = 1; + if(angle_delta < 1 && last_detected_cnt_ < 50) + { + gimbal_angle_.yaw_angle = yaw_raw_angle_; + } + else + { + gimbal_angle_.yaw_angle = angle_delta * 0.1 * 180.0/CV_PI + yaw_raw_angle_; + } + } + else + { + gimbal_angle_.yaw_mode = 0; + gimbal_angle_.yaw_angle = delta_yaw + yaw_raw_angle_; + } + gimbal_angle_.pitch_angle = delta_pitch; + gimbal_angle_.yaw_original = yaw_raw_angle_ + 10000.0; + Eigen::Vector3d pitch_reset, yaw_reset; + pitch_reset << 0, 0, 0; + yaw_reset << yaw_raw_angle_, 0, 0; + + pitch_kf_->Reset(pitch_reset); + yaw_kf_->Reset(yaw_reset); + detected_cnt_ = 0; + predict_time = 0.0; + } + + if(detected_enemy_ == 1) { + armor_detection_.detected_enemy = true; + } + else { + armor_detection_.detected_enemy = false; + } + + enemy_info_pub_.publish(gimbal_angle_); + + armor_detection_.yaw_angle = delta_yaw; + armor_detection_.gimbal_angle = yaw_ecd_angle_; + armor_detection_.distance = target_3d_.z; + armor_detection_pub_.publish(armor_detection_); + + // update last state + image_last_timestamp_ = image_timestamp_; + last_pitch_ = pitch; + last_yaw_ = yaw; + last_detected_enemy_ = detected_enemy_; + + std::cout << "miss_cnt: " << missed_cnt_ << std::endl; +} + +int main(int argc, char** argv) +{ + signal(SIGINT, SignalHandler); + signal(SIGTERM, SignalHandler); + ros::init(argc, argv, "robot_detection_node", ros::init_options::NoSigintHandler); + ros::NodeHandle ros_nh_; + sentry_enemy_.resize(2); + enemy_info_pub_ = ros_nh_.advertise("cmd_gimbal_angle", 1); + armor_detection_pub_ = ros_nh_.advertise("armor_detection_info", 1); + gimbal_angle_sub_ = ros_nh_.subscribe("gimbal_angle_info", 1, GimbalAngleCallback); + sentry_pose_sub_ = ros_nh_.subscribe("robot_zmq_pose", 1, SentryPoseCallback); + robot_pose_sub_ = ros_nh_.subscribe("eskf_pose", 1, EskfPoseCallback); + + image_transport::ImageTransport it(ros_nh_); + image_sub_ = it.subscribe("/mvsua_cam/image_raw1", 1, ImageCallback); + + //kalmanfilter init + pitch_kf_ = std::make_unique(); + yaw_kf_ = std::make_unique(); + pitch_kf_->Init(0.01, 5, 0.5, 0.0005); + yaw_kf_->Init(0.01, 5, 0.5, 0.0005); + + std::string robot_version, yaml_filename; + bool color, enable_debug; + double light_min_angle, light_max_angle_diff, light_aspect_ratio, armor_max_angle, armor_max_aspect_ratio, armor_min_aspect_ratio; + int blue_threshold, red_threshold, red_brightness_threshold, blue_brightness_threshold, robot_id; + cv::Mat intrinsic_matrix, distortion_coeffs; + ros_nh_.param("robot_id", robot_id, 2); + ros_nh_.getParam("robot_version", robot_version); + if(robot_version == "rmua") + yaml_filename = ros::package::getPath("robot_detection") + "/camera_param/camera_param_rmua" + std::to_string(robot_id) + ".yaml"; + else + yaml_filename = ros::package::getPath("robot_detection") + "/camera_param/camera_param_icra" + std::to_string(robot_id) + ".yaml"; + std::cout << yaml_filename.data() << std::endl; + cv::FileStorage fs(yaml_filename , cv::FileStorage::READ); + fs["cameraMatrix"] >> intrinsic_matrix; + fs["distCoeffs"] >> distortion_coeffs; + fs.release(); + ros_nh_.getParam("enemy_color", color_str); + if(color_str == "red") + color = true; + else if(color_str == "blue") + color = false; + ros_nh_.param("enable_debug", enable_debug, true); + ros_nh_.param("light_min_angle", light_min_angle, 60.0); + ros_nh_.param("light_max_angle_diff", light_max_angle_diff, 20.0); + ros_nh_.param("light_aspect_ratio", light_aspect_ratio, 1.5); + ros_nh_.param("armor_max_angle", armor_max_angle, 25.0); + ros_nh_.param("armor_max_aspect_ratio", armor_max_aspect_ratio, 3.0); + ros_nh_.param("armor_min_aspect_ratio", armor_min_aspect_ratio, 1.2); + ros_nh_.param("blue_threshold", blue_threshold, 100); + ros_nh_.param("red_threshold", red_threshold, 50); + ros_nh_.param("red_brightness_threshold", red_brightness_threshold, 55); + ros_nh_.param("blue_brightness_threshold", blue_brightness_threshold, 150); + armor_detector_.LoadParam(color, + enable_debug, + light_min_angle, + light_max_angle_diff, + light_aspect_ratio, + armor_max_angle, + armor_max_aspect_ratio, + armor_min_aspect_ratio, + blue_threshold, + red_threshold, + red_brightness_threshold, + blue_brightness_threshold, + intrinsic_matrix, distortion_coeffs); + + double gimbal_offset_x, gimbal_offset_y, gimbal_offset_z, bullet_init_v, bullet_init_k; + ros_nh_.param("gimbal_offset_x", gimbal_offset_x, 0.0); + ros_nh_.param("gimbal_offset_y", gimbal_offset_y, 160.0); + ros_nh_.param("gimbal_offset_z", gimbal_offset_z, 130.0); + ros_nh_.param("bullet_init_v", bullet_init_v, 16.0); + ros_nh_.param("bullet_init_k", bullet_init_k, 0.036); + if(robot_id == 1) bullet_init_k = 0.001; + else if(robot_id == 2) bullet_init_k = 0.036; + gimbal_control_.Init(gimbal_offset_x, gimbal_offset_y, gimbal_offset_z, bullet_init_v, bullet_init_k); + + ros::spin(); + return 0; +} \ No newline at end of file diff --git a/robot_detection/detection_node.h b/robot_detection/detection_node.h new file mode 100755 index 0000000..a68a33c --- /dev/null +++ b/robot_detection/detection_node.h @@ -0,0 +1,12 @@ +#ifndef DETECTION_NODE_H +#define DETECTION_NODE_H + +#include +#include "mvsua_camera.h" +#include +#include "gimbal_control.h" +#include "armor_detection.h" +#include "robot_msgs/GimbalAngle.h" +#include "robot_msgs/ArmorDetection.h" + +#endif \ No newline at end of file diff --git a/robot_detection/gimbal_control/gimbal_control.cpp b/robot_detection/gimbal_control/gimbal_control.cpp new file mode 100755 index 0000000..12c8796 --- /dev/null +++ b/robot_detection/gimbal_control/gimbal_control.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + + ***************************************************************************/ +#include +#include + +#include "gimbal_control.h" + +namespace robot_detection { + + void GimbalContrl::Init(double x, double y, double z, + double init_v, + double init_k) { + offset_.x = x; + offset_.y = y; + offset_.z = z; + init_v_ = init_v; + init_k_ = init_k; + } + + +//air friction is considered + double GimbalContrl::BulletModel(double x, double v, double angle) { //x:m,v:m/s,angle:rad + double t, y; + if (init_k_ == 0) ROS_WARN("init_k_ = 0! "); + if (v == 0) ROS_WARN("v = 0! "); + if (cos(angle) == 0) ROS_WARN("cos(angle) = 0! "); + t = (double) ((exp(init_k_ * x) - 1) / (init_k_ * v * cos(angle))); + y = (double) (v * sin(angle) * t - GRAVITY * t * t / 2); + return y; + } + +//x:distance , y: height + double GimbalContrl::GetPitch(double x, double y, double v) { + double y_temp, y_actual, dy; + double a; + y_temp = y; + // by iteration + for (int i = 0; i < 20; i++) { + a = (double) atan2(y_temp, x); + y_actual = BulletModel(x, v, a); + dy = y - y_actual; + y_temp = y_temp + dy; + if (fabsf(dy) < 0.001) { + break; + } + } + return a; + + } + + void GimbalContrl::Transform(cv::Point3f &postion, double &pitch, double &yaw) { + pitch = -GetPitch((postion.z + offset_.z) / 1000, -(postion.y + offset_.y) / 1000, init_v_) * 180 / M_PI; + //yaw positive direction :anticlockwise + yaw = -(double) (atan2(postion.x + offset_.x, postion.z + offset_.z)) * 180 / M_PI; + } + +} // roborts_detection + + + diff --git a/robot_detection/gimbal_control/gimbal_control.h b/robot_detection/gimbal_control/gimbal_control.h new file mode 100755 index 0000000..52dbf7a --- /dev/null +++ b/robot_detection/gimbal_control/gimbal_control.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + ***************************************************************************/ +#ifndef ROBORTS_DETECTION_GIMBAL_CONTROL_H +#define ROBORTS_DETECTION_GIMBAL_CONTROL_H + +#include +#include +#include + +namespace robot_detection { + const double GRAVITY = 9.78; + +/** + * @brief The class can make a transformation: the 3D position of enemy --> pitch,yaw angle of gimbal. + * For more derails, see projectile_model.pdf + * TODO: add enemy motion estimation + */ + + class GimbalContrl { + private: + /** + * @brief Calculate the actual y value with air resistance + * @param x the distanc + * @param v Projectile velocity + * @param angle Pitch angle + * @return The actual y value in the gimbal coordinate + */ + double BulletModel(double x, double v, double angle); + + /** + * @brief Get the gimbal control angle + * @param x Distance from enemy(the armor selected to shoot) to gimbal + * @param y Value of y in gimbal coordinate. + * @param v Projectile velocity + * @return Gimbal pitch angle + */ + double GetPitch(double x, double y, double v); + + public: + /** + * @brief Init the Transformation matrix from camera to gimbal //TODO: write in ros tf + * @param x Translate x + * @param y Translate y + * @param z Translate z + * @param pitch Rotate pitch + * @param yaw Rotate yaw + */ + void Init(double x, double y, double z, + double init_v = 20, + double init_k = 0.036 + ); + + /** + * @brief Get the gimbal control info. + * @param postion Enemy position(actually it should be the target armor). + * @param pitch Input and output actual pitch angle + * @param yaw Input and output actual yaw angle + */ + void Transform(cv::Point3f &postion, double &pitch, double &yaw); + + void set_velocity(double speed) + { + init_v_ = speed; + } + + public: + //! Transformation matrix between camera coordinate system and gimbal coordinate system. + //! Translation unit: cm + cv::Point3f offset_; + + //! Initial value + double init_v_; + double init_k_; + + }; + +} + +#endif diff --git "a/robot_detection/gimbal_control/\345\274\271\351\201\223\346\250\241\345\236\213.pdf" "b/robot_detection/gimbal_control/\345\274\271\351\201\223\346\250\241\345\236\213.pdf" new file mode 100755 index 0000000..026e19f Binary files /dev/null and "b/robot_detection/gimbal_control/\345\274\271\351\201\223\346\250\241\345\236\213.pdf" differ diff --git a/robot_detection/kalman_filter/CMakeLists.txt b/robot_detection/kalman_filter/CMakeLists.txt new file mode 100755 index 0000000..b190229 --- /dev/null +++ b/robot_detection/kalman_filter/CMakeLists.txt @@ -0,0 +1,20 @@ +project(kalman_filter) + +find_package(Eigen3 REQUIRED) + +add_library(kalman_filter + SHARED + kalman_filter.cpp + ) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ) + +target_include_directories(${PROJECT_NAME} + PUBLIC + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ) diff --git a/robot_detection/kalman_filter/kalman_filter.cpp b/robot_detection/kalman_filter/kalman_filter.cpp new file mode 100755 index 0000000..a9f7526 --- /dev/null +++ b/robot_detection/kalman_filter/kalman_filter.cpp @@ -0,0 +1,171 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of  + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + ***************************************************************************/ + +#include "kalman_filter.h" + +namespace robot_detection { + +KalmanFilter::KalmanFilter() +{ + +} + +void KalmanFilter::Init(double dT, double alpha, double sigma_a2, double R0) +{ + alpha_ = alpha; + sigma_a2_= sigma_a2; + + nominal_dT_ = dT; + F_ << 1, dT, (alpha_*dT + std::exp(-alpha_*dT) - 1) / (pow(alpha_, 2)), + 0, 1, (1 - std::exp(-alpha_*dT))/alpha_, + 0, 0, std::exp(-alpha_*dT); + + H_ << 1, 0, 0; + R_offSet_ = R0; + P_offset_ << R0, 0, 0, + 0, R0/dT, 0, + 0, 0, R0/dT/dT; + + double q11 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT) + 2*alpha_*dT + 2/3*pow(alpha_, 3)*pow(dT, 3) - 2*pow(alpha_, 2)*pow(dT, 2) - 4*alpha_*dT*exp(-alpha_*dT)) / (2*pow(alpha_, 5)); + double q12 = 2*alpha_*sigma_a2_*(1 + std::exp(-2*alpha_*dT) - 2*std::exp(-alpha_*dT) + 2*alpha_*dT*exp(-alpha_*dT) - 2*alpha_*dT + pow(alpha_, 2)*pow(dT, 2)) / (2*pow(alpha_, 4)); + double q13 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT) - 2*alpha_*dT*std::exp(-alpha_*dT)) / (2*pow(alpha_, 3)); + double q22 = 2*alpha_*sigma_a2_*(4*std::exp(-alpha_*dT) - 3 - std::exp(-2*alpha_*dT) + 2*alpha_*dT) / (2*pow(alpha_, 3)); + double q23 = 2*alpha_*sigma_a2_*(1 + std::exp(-2*alpha_*dT) - 2*std::exp(-alpha_*dT)) / (2*pow(alpha_, 2)); + double q33 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT)) / (2*alpha_); + + Q_offset_ << q11, q12, q13, + q12, q22, q23, + q13, q23, q33; + +} + +void KalmanFilter::Reset(Eigen::Vector3d& x) +{ + x_ = x; + P_ = P_offset_; + R_ = R_offSet_; + is_tracking_ = true; +} + +void KalmanFilter::SetState(Eigen::Vector3d& x) +{ + x_ = x; +} + +void KalmanFilter::StateUpdate(double dT, bool flag) +{ + if(flag) { + F_ << 1, dT, (alpha_*dT + std::exp(-alpha_*dT) - 1) / (pow(alpha_, 2)), + 0, 1, (1 - std::exp(-alpha_*dT))/alpha_, + 0, 0, std::exp(-alpha_*dT); + + double q11 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT) + 2*alpha_*dT + 2/3*pow(alpha_, 3)*pow(dT, 3) - 2*pow(alpha_, 2)*pow(dT, 2) - 4*alpha_*dT*exp(-alpha_*dT)) / (2*pow(alpha_, 5)); + double q12 = 2*alpha_*sigma_a2_*(1 + std::exp(-2*alpha_*dT) - 2*std::exp(-alpha_*dT) + 2*alpha_*dT*exp(-alpha_*dT) - 2*alpha_*dT + pow(alpha_, 2)*pow(dT, 2)) / (2*pow(alpha_, 4)); + double q13 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT) - 2*alpha_*dT*std::exp(-alpha_*dT)) / (2*pow(alpha_, 3)); + double q22 = 2*alpha_*sigma_a2_*(4*std::exp(-alpha_*dT) - 3 - std::exp(-2*alpha_*dT) + 2*alpha_*dT) / (2*pow(alpha_, 3)); + double q23 = 2*alpha_*sigma_a2_*(1 + std::exp(-2*alpha_*dT) - 2*std::exp(-alpha_*dT)) / (2*pow(alpha_, 2)); + double q33 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT)) / (2*alpha_); + + Q_ << q11, q12, q13, + q12, q22, q23, + q13, q23, q33; + + x_ = F_*x_; + P_ = F_*P_*(F_.transpose()) + Q_; + } + else { + x_.y() = 0.0; + x_.z() = 0.0; + + F_ << 1, dT, (alpha_*dT + std::exp(-alpha_*dT) - 1) / (pow(alpha_, 2)), + 0, 1, (1 - std::exp(-alpha_*dT))/alpha_, + 0, 0, std::exp(-alpha_*dT); + + double q11 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT) + 2*alpha_*dT + 2/3*pow(alpha_, 3)*pow(dT, 3) - 2*pow(alpha_, 2)*pow(dT, 2) - 4*alpha_*dT*exp(-alpha_*dT)) / (2*pow(alpha_, 5)); + double q12 = 2*alpha_*sigma_a2_*(1 + std::exp(-2*alpha_*dT) - 2*std::exp(-alpha_*dT) + 2*alpha_*dT*exp(-alpha_*dT) - 2*alpha_*dT + pow(alpha_, 2)*pow(dT, 2)) / (2*pow(alpha_, 4)); + double q13 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT) - 2*alpha_*dT*std::exp(-alpha_*dT)) / (2*pow(alpha_, 3)); + double q22 = 2*alpha_*sigma_a2_*(4*std::exp(-alpha_*dT) - 3 - std::exp(-2*alpha_*dT) + 2*alpha_*dT) / (2*pow(alpha_, 3)); + double q23 = 2*alpha_*sigma_a2_*(1 + std::exp(-2*alpha_*dT) - 2*std::exp(-alpha_*dT)) / (2*pow(alpha_, 2)); + double q33 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT)) / (2*alpha_); + + Q_ << q11, q12, q13, + q12, q22, q23, + q13, q23, q33; + + x_ = F_*x_; + P_ = F_*P_*(F_.transpose()) + Q_; + } +} + +void KalmanFilter::PredictWithVelocity(double dT) +{ + F_ << 1, dT, 0.0, + 0, 1, 0.0, + 0, 0, 1.0; + + double q11 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT) + 2*alpha_*dT + 2/3*pow(alpha_, 3)*pow(dT, 3) - 2*pow(alpha_, 2)*pow(dT, 2) - 4*alpha_*dT*exp(-alpha_*dT)) / (2*pow(alpha_, 5)); + double q12 = 2*alpha_*sigma_a2_*(1 + std::exp(-2*alpha_*dT) - 2*std::exp(-alpha_*dT) + 2*alpha_*dT*exp(-alpha_*dT) - 2*alpha_*dT + pow(alpha_, 2)*pow(dT, 2)) / (2*pow(alpha_, 4)); + double q13 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT) - 2*alpha_*dT*std::exp(-alpha_*dT)) / (2*pow(alpha_, 3)); + double q22 = 2*alpha_*sigma_a2_*(4*std::exp(-alpha_*dT) - 3 - std::exp(-2*alpha_*dT) + 2*alpha_*dT) / (2*pow(alpha_, 3)); + double q23 = 2*alpha_*sigma_a2_*(1 + std::exp(-2*alpha_*dT) - 2*std::exp(-alpha_*dT)) / (2*pow(alpha_, 2)); + double q33 = 2*alpha_*sigma_a2_*(1 - std::exp(-2*alpha_*dT)) / (2*alpha_); + + Q_ << q11, q12, q13, + q12, q22, q23, + q13, q23, q33; + + x_ = F_*x_; + P_ = F_*P_*(F_.transpose()) + Q_; +} + +void KalmanFilter::MeasurementUpdate(double z, bool flag) +{ + if(flag) { + Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); + z_ = z; + P_ = (H_.transpose() * (1 / R_) * H_ + P_.inverse()).inverse(); + K_ = P_*H_.transpose() * (1 / R_); + x_ = x_ + K_*(z_ - H_*x_); + } + else { + Eigen::Vector3d x_hat = x_; + Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); + z_ = z; + P_ = (H_.transpose() * (1 / R_) * H_ + P_.inverse()).inverse(); + K_ = P_*H_.transpose() * (1 / R_); + x_ = x_ + K_*(z_ - H_*x_); + x_.tail<2>() = x_hat.tail<2>(); + } + +} + +Eigen::Vector3d KalmanFilter::GetState() +{ + return x_; +} + +Eigen::Vector3d KalmanFilter::GetPredictState(double dT) +{ + Eigen::Matrix3d transform; + transform << 1, dT, 0.5*dT*dT, + 0, 1, dT, + 0, 0, 1; + return transform * x_; +} + + +} \ No newline at end of file diff --git a/robot_detection/kalman_filter/kalman_filter.h b/robot_detection/kalman_filter/kalman_filter.h new file mode 100755 index 0000000..cd76d83 --- /dev/null +++ b/robot_detection/kalman_filter/kalman_filter.h @@ -0,0 +1,58 @@ +#ifndef KALMAN_FILTER_H +#define KALMAN_FILTER_H + +#include +#include +#include +#include +#include +#include + +namespace robot_detection { + +class KalmanFilter +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + KalmanFilter(); + + void Init(double dT, double alpha, double sigma_a2, double R0); + + void Reset(Eigen::Vector3d& x); + + void SetState(Eigen::Vector3d& x); + + void StateUpdate(double dT, bool flag); + + void PredictWithVelocity(double dT); + + void MeasurementUpdate(double z, bool flag); + + Eigen::Vector3d GetState(); + + Eigen::Vector3d GetPredictState(double dT); + + Eigen::Vector3d x_; + double z_; + + Eigen::Matrix3d F_; + Eigen::Matrix3d P_; + Eigen::Matrix H_; + Eigen::Matrix3d Q_; + double R_; + Eigen::Matrix K_; + Eigen::Matrix3d P_offset_; + Eigen::Matrix3d Q_offset_; + double R_offSet_; + + + double alpha_; + double sigma_a2_; + double nominal_dT_; + bool is_tracking_ = false; + bool is_skip_ = false; +}; + +} + +#endif diff --git a/robot_detection/launch/robot_detection.launch b/robot_detection/launch/robot_detection.launch new file mode 100755 index 0000000..d58b1b6 --- /dev/null +++ b/robot_detection/launch/robot_detection.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robot_detection/launch/robot_detection_test.launch b/robot_detection/launch/robot_detection_test.launch new file mode 100755 index 0000000..d9501e0 --- /dev/null +++ b/robot_detection/launch/robot_detection_test.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robot_detection/package.xml b/robot_detection/package.xml new file mode 100755 index 0000000..6f58f15 --- /dev/null +++ b/robot_detection/package.xml @@ -0,0 +1,59 @@ + + + robot_detection + 0.0.0 + The robot_camera package + + + + + uncle + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + robot_msgs + + + + + + + diff --git "a/robot_detection/picture/\345\215\241\345\260\224\346\233\274\346\273\244\346\263\242\345\231\250.png" "b/robot_detection/picture/\345\215\241\345\260\224\346\233\274\346\273\244\346\263\242\345\231\250.png" new file mode 100755 index 0000000..c8f310b Binary files /dev/null and "b/robot_detection/picture/\345\215\241\345\260\224\346\233\274\346\273\244\346\263\242\345\231\250.png" differ diff --git "a/robot_detection/picture/\345\215\241\345\260\224\346\233\274\346\273\244\346\263\242\346\225\210\346\236\234.png" "b/robot_detection/picture/\345\215\241\345\260\224\346\233\274\346\273\244\346\263\242\346\225\210\346\236\234.png" new file mode 100755 index 0000000..6d8ed1f Binary files /dev/null and "b/robot_detection/picture/\345\215\241\345\260\224\346\233\274\346\273\244\346\263\242\346\225\210\346\236\234.png" differ diff --git "a/robot_detection/picture/\345\274\271\351\201\223\346\250\241\345\236\213.pdf" "b/robot_detection/picture/\345\274\271\351\201\223\346\250\241\345\236\213.pdf" new file mode 100755 index 0000000..026e19f Binary files /dev/null and "b/robot_detection/picture/\345\274\271\351\201\223\346\250\241\345\236\213.pdf" differ diff --git "a/robot_detection/picture/\345\274\271\351\201\223\346\250\241\345\236\213.png" "b/robot_detection/picture/\345\274\271\351\201\223\346\250\241\345\236\213.png" new file mode 100755 index 0000000..254b6fa Binary files /dev/null and "b/robot_detection/picture/\345\274\271\351\201\223\346\250\241\345\236\213.png" differ diff --git "a/robot_detection/picture/\347\254\254\344\270\200\344\272\272\347\247\260.gif" "b/robot_detection/picture/\347\254\254\344\270\200\344\272\272\347\247\260.gif" new file mode 100755 index 0000000..7f357b0 Binary files /dev/null and "b/robot_detection/picture/\347\254\254\344\270\200\344\272\272\347\247\260.gif" differ diff --git "a/robot_detection/picture/\350\200\227\346\227\266.png" "b/robot_detection/picture/\350\200\227\346\227\266.png" new file mode 100755 index 0000000..6646365 Binary files /dev/null and "b/robot_detection/picture/\350\200\227\346\227\266.png" differ diff --git "a/robot_detection/picture/\350\207\252\347\236\204\346\230\276\347\244\272\345\261\217.gif" "b/robot_detection/picture/\350\207\252\347\236\204\346\230\276\347\244\272\345\261\217.gif" new file mode 100755 index 0000000..b43a8f6 Binary files /dev/null and "b/robot_detection/picture/\350\207\252\347\236\204\346\230\276\347\244\272\345\261\217.gif" differ diff --git "a/robot_detection/picture/\350\243\205\347\224\262\346\235\277\350\257\206\345\210\253\346\225\210\346\236\234.png" "b/robot_detection/picture/\350\243\205\347\224\262\346\235\277\350\257\206\345\210\253\346\225\210\346\236\234.png" new file mode 100755 index 0000000..fb9ea7c Binary files /dev/null and "b/robot_detection/picture/\350\243\205\347\224\262\346\235\277\350\257\206\345\210\253\346\225\210\346\236\234.png" differ diff --git "a/robot_detection/picture/\350\243\205\347\224\262\346\235\277\350\257\206\345\210\253\346\265\201\347\250\213.png" "b/robot_detection/picture/\350\243\205\347\224\262\346\235\277\350\257\206\345\210\253\346\265\201\347\250\213.png" new file mode 100755 index 0000000..f137c52 Binary files /dev/null and "b/robot_detection/picture/\350\243\205\347\224\262\346\235\277\350\257\206\345\210\253\346\265\201\347\250\213.png" differ diff --git "a/robot_detection/picture/\350\275\246\345\255\220\345\233\276\347\211\207.png" "b/robot_detection/picture/\350\275\246\345\255\220\345\233\276\347\211\207.png" new file mode 100755 index 0000000..ff6137c Binary files /dev/null and "b/robot_detection/picture/\350\275\246\345\255\220\345\233\276\347\211\207.png" differ diff --git "a/robot_detection/picture/\350\276\205\345\212\251\347\236\204\345\207\206.png" "b/robot_detection/picture/\350\276\205\345\212\251\347\236\204\345\207\206.png" new file mode 100755 index 0000000..3c1b087 Binary files /dev/null and "b/robot_detection/picture/\350\276\205\345\212\251\347\236\204\345\207\206.png" differ