diff --git a/README.md b/README.md
index c8467d39e45fa74b13b87cc42442542293316598..981a14d620f246ad40eebcae050d89e4fb2579d0 100644
--- a/README.md
+++ b/README.md
@@ -35,6 +35,14 @@ sudo cp $(pwd)/hnurm_camera/lib/linux/* /usr/lib/
#下载nomachine到/usr目录并解压安装
wget -P ~/Downloads/ https://download.nomachine.com/download/8.11/Linux/nomachine_8.11.3_4_amd64.deb
sudo dpkg --install ~/Downloads/nomachine*amd64.deb
+
+#其他依赖项
+sudo apt -y install ros-humble-sophus
+sudo apt -y install ros-humble-libg2o
+sudo apt -y install ros-humble-camera-info-manager
+sudo apt -y install ros-camera-calibration
+sudo apt -y install ros-humble-camera-calibration
+rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
```
4. 安装ROS2 humble
@@ -84,7 +92,7 @@ echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
关于组局域网连接nomachine的方法原理的进一步阐释,可以参考[这里](https://blog.csdn.net/qq_50791664/article/details/135578810)。
-6. 创建路径`/home/rm/hnuvision/src`,将本项目目录下文件复制或剪切到这个路径,然后使用`colcon build`编译,如果出现依赖问题,再补充安装即可。
+6. 创建路径`/home/rm/hnuvision/src`,将本项目目录下所有文件(包括隐藏文件)复制或剪切到这个路径,然后使用`colcon build`编译,编译失败多半是依赖没装好,回到之前再补充安装即可。
7. 设置自启动
在启动应用程序中添加
@@ -104,35 +112,37 @@ sudo usermod -aG dialout rm
# 概览
## 功能包
-本框架基于ROS平台设计了5个功能包,如下:
+本框架基于ROS平台设计了6个功能包,如下:
1. hnurm_uart:负责与下位机的通讯
-2. hnurm_camera:调用相机,感知外界信息
+2. hnurm_camera:调用相机
3. hnurm_detect:检测装甲
4. hnurm_armor:预测装甲的运动并进行补偿
-5. hnurm_bringup:启动其他全部节点并适配双云台,对于单云台,默认相机为`right`
+5. hnurm_bringup:启动其他全部节点并适配双云台
+6. hnurm_interfaces:存储所有消息类型
## 自瞄流程
-1. 从相机获取图像,图像接受线程获取图像。
-2. 图像处理线程开始进行识别和预测。识别过程不需要接受串口数据,预测需要。识别过程首先计算目标的像素坐标系下的坐标,然后通过PnP算法计算出目标与的相对位置坐标,然后通过预测算法预测出目标对于相机的相对坐标位置、空间位姿、移动速度、半径。
-3. 将预测的俯仰角和偏航角发送到串口
+1. 启动`hnurm_uart`节点,可以自动识别串口名字并尝试读写数据。
+2. 启动`hnurm_camera`节点,可以根据参数文件中的相机id打开相机获取图像。
+3. 启动`hnurm_detect`节点,可以对相机图像进行装甲板的提取。
+4. 启动`hnurm_armor`节点,可以装甲板进行预测和弹道的计算,并将计算结果发送给`hnurm_uart`。
+5. 启动`hnurm_bringup`功能包中的`compose_node`节点,可以一次性启动如上所有节点。对于哨兵这样的双云台机器人来说,需要
# 标定脚本执行方法
-1. 准备好标定板,根据标定板的方格数量、方格面积、方格类型修改`hnurm_camera/CameraCalibration/default.xml`文件。
-```xml
-
-7
-7
-
-20
-"CIRCLES_GRID"
+1. 准备好标定板,根据标定板的方格数量、方格面积、方格类型修改`hnurm_camera/launch/calib.launch.py`文件。
+```python
+ # execute `ros2 run camera_calibration cameracalibrator --size --square
+ camera_calibration_node = ExecuteProcess(
+
+ cmd=['ros2', 'run', 'camera_calibration', 'cameracalibrator', '--size', '11x8', '--square', '0.03',
+ '--no-service-check'
+ ],
+ output='screen'
+ )
```
-2. 编译项目`CameraCalibration`并执行,clion中需要指定`Program arguments`为`../default.xml`。在程序执行过程中,按下`G`键开始标定,手持标定板,在图像四个角周围移动标定板。
-3. 完成拍摄后,程序会打印出loss(误差值)并自动保存文件,一般认为loss低于0.2是可以接受的。
-4. 最后检查一下图像的四周是否存在肉眼可见的畸变,然后就可以退出程序了。
+2. 运行此文件,移动标定板进行标定。标定距离不要超过2m;标定时,标定板俯仰角上下15°左右,偏航角不限;图像边缘也要照顾到。
+3. 标定文件存储在`/tmp`下,解压缩并打开文件,把相机内参矩阵手动复制到`hnurm_aromor/data`下任意一个标定文件下,并顺便依据相机id修改标定文件名字。
-# 路径问题
-在运行程序前,修改相机id(位于`vision.right.launch.py`)和神经网络模型(位于`hnurm_detect/params/default.xml`)的路径。
# 串口问题
## 解包顺序
@@ -145,8 +155,8 @@ y对应roll,z对应yaw,x对应pitch;
# 装甲板识别问题
## 灯条识别正常,但是没有构成装甲板
-查看config.yaml中ignore_classes参数,可能因为是识别成`negative`被筛除了
-```shell
+- 查看`hnurm_detect/params/default.yaml`中ignore_classes参数,可能因为是识别成`negative`被筛除了
+```yaml
ignore_classes:
- "2"
# - negative
@@ -154,18 +164,11 @@ y对应roll,z对应yaw,x对应pitch;
# - base
```
-**注意,yaml的格式相当严格,如下格式的yaml无法运行:**
-```shell
- ignore_classes:
-# - negative
-# - outpost
-# - base
- - "2"
-```
+- `hnurm_detect/src/detector.cpp`中的`isArmor`函数把它筛去了,调整参数试试。
## 解决数字识别网络的误识别问题:
1. 在适当的距离重新对焦并标定
-2. 调整光圈到最大,调整CameraParam.yaml的曝光时间和增益
+2. 调整光圈,调整CameraParam.yaml的曝光时间和增益
3. 调整二值化阈值
@@ -183,7 +186,7 @@ static constexpr float LARGE_ARMOR_WIDTH = 225;
static constexpr float LARGE_ARMOR_HEIGHT = 55;
```
-2. 检查相机成像是否清晰(即焦距是否合适),光圈是否调整到最大。**每次调整完焦距或者光圈后都需要重新标定**。
+2. 检查相机成像是否清晰(即焦距是否合适),光圈是否调整合适。**每次调整完焦距或者光圈后都需要重新标定**。
3. 检查`hnurm_armor/params/default.yaml`中的相机偏移参数是否正确。应该设置为相机光心在惯性传感器坐标系下的坐标:相机镜头方向为y轴正方向,天空为z轴正方向。
```yaml
@@ -207,7 +210,7 @@ static constexpr float LARGE_ARMOR_HEIGHT = 55;
# 预测问题
## 查看channel_delay是否设置正确
-channel_delay这个时间定义为电控受到开火指令后转动拨盘的延迟,这个参数会严重的影响预测的准度。对于步兵而言,比赛时几乎都是连发,可以设置为0.01;对于英雄而言,就需要去询问电控组的同学。此外,调整方式如下:
+channel_delay这个时间定义为电控受到开火指令后转动拨盘的延迟,这个参数会严重的影响预测的准度。对于步兵而言,比赛时几乎都是连发,可以设置为0.05;对于英雄而言,就需要去询问电控组的同学。此外,调整方式如下:
1. 当弹道晚于目标,那么channel_delay过小,程序认为子弹几乎可以立即发射,但现实是子弹发射延迟更大,所以弹道慢了。应该增大channel_delay
2. 弹道早于目标,应该减小channel_delay
@@ -215,7 +218,7 @@ channel_delay这个时间定义为电控受到开火指令后转动拨盘的延
预测的角速度是能否击中目标的关键,这通常与曝光时间和增益有所关联。曝光时间增加可以获得更多颜色信息,但会降低帧率。
# 完整调试流程
-1. 光圈最大,对焦并标定(四个角不要有畸变),对应地方修改相机id
+1. 调整光圈、焦距,标定(四个角不要有畸变),对应地方修改相机id和参数(宽高、像素格式)
2. 根据比赛场地调整曝光和增益,主要是曝光
3. 调整装甲板识别参数、相机光心与惯性传感器的相对位置、摩擦轮中心与惯性传感器的相对位置
4. 静止击打装甲板确保可以正中,主要调整bias_yaw。bias_pitch默认就是0,几乎不用调整,如果pitch不准,优先去调整摩擦系数,实在不行的话再去调bias_pitch
@@ -232,3 +235,5 @@ sudo apt install ros-$ROS_DISTRO-foxglove-bridge
$ ros2 launch foxglove_bridge foxglove_bridge_launch.xml
```
3. 打开应用即可
+
+
diff --git a/bringup.sh b/bringup.sh
index 0b56eba19afafd6b0e146e3d342483c9baee1d4e..341bb492d2756d664cea7f7f51c6bf322a39d43c 100644
--- a/bringup.sh
+++ b/bringup.sh
@@ -1,9 +1,16 @@
#!/bin/bash
+#while [ true ]; do
+# if ls /dev/ttyACM* 1> /dev/null 2>&1 || ls /dev/ttyUSB* 1> /dev/null 2>&1 ;then
+# break
+# else
+# echo "waiting for the uart..."
+# fi
+#done
+
cmds=(
- "ros2 launch hnurm_bringup hnurm_bringup.launch.py"
- "ros2 launch hnurm_bringup vision.right.launch.py"
+ "ros2 launch hnurm_bringup compose.launch.py"
)
for cmd in "${cmds[@]}"
do
diff --git a/hnurm_armor/CMakeLists.txt b/hnurm_armor/CMakeLists.txt
index a7bf102113692dbb5d2fc7d8353cf932cf4fe8ef..b759e122fa79f2461b9249397ac33511a99c2e3c 100644
--- a/hnurm_armor/CMakeLists.txt
+++ b/hnurm_armor/CMakeLists.txt
@@ -3,84 +3,23 @@ project(hnurm_armor)
set(CMAKE_CXX_STANDARD 17)
-find_package(rosidl_default_generators REQUIRED)
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(cv_bridge REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(angles REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
-find_package(tf2 REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(tf2_eigen REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-
-find_package(visualization_msgs REQUIRED)
-
-find_package(Eigen3 REQUIRED)
-find_package(OpenCV REQUIRED)
-find_package(Sophus REQUIRED)
-find_package(g2o REQUIRED)
-
-find_package(hnurm_uart REQUIRED)
-find_package(hnurm_detect REQUIRED)
-
-set(dependencies
- rclcpp
- Eigen3
- OpenCV
- cv_bridge
- sensor_msgs
- angles
- hnurm_uart
- hnurm_detect
- visualization_msgs
- tf2
- tf2_ros
- tf2_eigen
- tf2_geometry_msgs
-)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
- msg/Target.msg
- msg/TrackerInfo.msg
- DEPENDENCIES std_msgs geometry_msgs sensor_msgs
-)
-rosidl_get_typesupport_target(cpp_typesupport_target
- ${PROJECT_NAME} rosidl_typesupport_cpp)
-
-add_library(${PROJECT_NAME}_lib SHARED
- src/extended_kalman_filter.cpp src/Processor.cpp src/tracker.cpp
+ament_auto_add_library(hnurm_armor
+ src/armor_node.cpp
src/Compensator.cpp
-)
-target_include_directories(${PROJECT_NAME}_lib PUBLIC
- $
- $
-)
-ament_target_dependencies(${PROJECT_NAME}_lib ${dependencies})
-target_link_libraries(${PROJECT_NAME}_lib
- Eigen3::Eigen
- Sophus::Sophus
- g2o::core
- ${cpp_typesupport_target}
+ src/extended_kalman_filter.cpp
+ src/Processor.cpp
+ src/tracker.cpp
)
-add_executable(${PROJECT_NAME}_node src/armor_node.cpp src/main.cpp src/solvePnPBA.cpp)
-target_include_directories(${PROJECT_NAME}_node PUBLIC
- $
- $)
-ament_target_dependencies(${PROJECT_NAME}_node ${dependencies})
-target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_lib ${cpp_typesupport_target})
+ament_auto_add_executable(hnurm_armor_node src/main.cpp)
+target_link_libraries(hnurm_armor_node hnurm_armor)
-install(TARGETS ${PROJECT_NAME}_lib
- DESTINATION lib
-)
-install(TARGETS ${PROJECT_NAME}_node
- DESTINATION lib/${PROJECT_NAME}
+ament_auto_package(
+ INSTALL_TO_SHARE
+ launch
+ params
+ data
)
-install(DIRECTORY launch params data
- DESTINATION share/${PROJECT_NAME}
-)
-
-
-ament_package()
diff --git a/hnurm_armor/data/2BDF67015843.yaml b/hnurm_armor/data/2BDF67015843.yaml
old mode 100644
new mode 100755
index a05533e79a83736d2c60d8783c3cd8b6fcd60086..6cd1dc1330338f65dda9a3dbff97bc841d6cba54
--- a/hnurm_armor/data/2BDF67015843.yaml
+++ b/hnurm_armor/data/2BDF67015843.yaml
@@ -1,6 +1,6 @@
%YAML:1.0
---
-calibration_time: "Tue 26 Mar 2024 10:15:41 PM CST"
+calibration_time: "2024年04月10日 星期三 05时33分29秒"
# flags: +fix_aspectRatio +fix_principal_point +zero_tangent_dist +fix_k4 +fix_k5
flags: 6158
fisheye_model: 0
@@ -8,11 +8,11 @@ camera_matrix: !!opencv-matrix
rows: 3
cols: 3
dt: d
- data: [ 8.6285852023350242e+02, 0., 6.3950000000000000e+02, 0.,
- 8.6285852023350242e+02, 3.5950000000000000e+02, 0., 0., 1. ]
+ data: [ 8.6204018260781015e+02, 0., 6.3950000000000000e+02, 0.,
+ 8.6204018260781015e+02, 3.5950000000000000e+02, 0., 0., 1. ]
distortion_coefficients: !!opencv-matrix
rows: 5
cols: 1
dt: d
- data: [ -9.9345144087277501e-02, 1.2013852036217572e-01, 0., 0.,
- -4.4704145076911242e-02 ]
+ data: [ -9.7539644997920810e-02, 1.0768468716235712e-01, 0., 0.,
+ -2.7325839492794737e-02 ]
diff --git a/hnurm_armor/include/hnurm_armor/Compensator.h b/hnurm_armor/include/hnurm_armor/Compensator.h
index 5b0d5ca271507ff5dacfa97ce4456d6d0dd0aec8..8304cd463943fb7d08b0c07b16c18eb2ae99ace9 100755
--- a/hnurm_armor/include/hnurm_armor/Compensator.h
+++ b/hnurm_armor/include/hnurm_armor/Compensator.h
@@ -2,8 +2,10 @@
#define COMPENSATOR_H
#include "hnurm_armor/DataType.hpp"
-#include
-#include
+
+#include
+#include
+
#include
#include
#include
@@ -11,38 +13,63 @@
namespace hnurm
{
-class Compensator
-{
-private:
- // 默认参数
- float gravity_ = 9.8;
- float friction_coeff_ = 0.02;
- float bullet_speed_bias_ = -0.5;
- float imutofrt_y = 0.0;
- float imutofrt_z = 0.0;
- TargetInfo t_info;
- float fx, fy, fz,fx_center,fy_center;
- float pout, yout;
- float tmp_fly_time;
- float bullet_speed_;
- float channel_delay_;
- void PredictPose(ArmorsNum &armors_num);
-
- void CompensatePitch();
- [[nodiscard]] double diff_eq(double v) const;
- double integrate_euler(double v0, double t);
-public:
- explicit Compensator(rclcpp::Node::SharedPtr node);
-
- float CalcFinalAngle(
- TargetInfo &target_msg, hnurm_uart::msg::VisionRecvData &recv_data, hnurm_uart::msg::VisionSendData &send_data, ArmorsNum &num
- );
- static bool cmp(const cv::Point3f &a, const cv::Point3f &b);
- float yaw_bias_ = 0.0;
- float pitch_bias_ = 0.0;
-private:
- rclcpp::Node::SharedPtr node_;
-};
+ class Compensator
+ {
+ private:
+ // 默认参数
+ float gravity_ = 9.8;
+ float friction_coeff_ = 0.02;
+ float bullet_speed_bias_ = -0.5;
+ float imutofrt_y = 0.0;
+ float imutofrt_z = 0.0;
+ TargetInfo t_info;
+ float fx, fy, fz, fx_center, fy_center, fv, yaw_difference;
+ float pout, yout;
+ float tmp_fly_time;
+ float bullet_speed_;
+ float channel_delay_;
+ std::vector px;
+ std::vector py;
+ std::vector pz;
+ std::vector pv;
+
+ void PredictPose(ArmorsNum &armors_num);
+
+ void PredictPosition();
+
+ void CompensatePitch();
+
+ float calculateAverage(const std::vector &vec);
+
+ void Averagedistance(std::vector &p, float value);
+
+ [[nodiscard]] double diff_eq(double v) const;
+
+ double integrate_euler(double v0, double t);
+
+ public:
+ explicit Compensator(rclcpp::Node::SharedPtr node);
+
+ float CalcFinalAngle(
+ TargetInfo &target_msg,
+ hnurm_interfaces::msg::VisionRecvData &recv_data,
+ hnurm_interfaces::msg::VisionSendData &send_data,
+ ArmorsNum &num
+ );
+
+ struct predict_armor
+ {
+ cv::Point3f position;
+ float yaw;
+ };
+
+ static bool cmp(const predict_armor &a, const predict_armor &b);
+
+ float yaw_bias_ = 0.0;
+ float pitch_bias_ = 0.0;
+ private:
+ rclcpp::Node::SharedPtr node_;
+ };
} // namespace hnurm
#endif // COMPENSATOR_H
\ No newline at end of file
diff --git a/hnurm_armor/include/hnurm_armor/DataBuffer.hpp b/hnurm_armor/include/hnurm_armor/DataBuffer.hpp
deleted file mode 100755
index 1869e042ebf87a7f054503a846f63874959e82cf..0000000000000000000000000000000000000000
--- a/hnurm_armor/include/hnurm_armor/DataBuffer.hpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/* kiko@idiospace.com 2020.01.20 */
-
-#ifndef DataBuffer_HPP
-#define DataBuffer_HPP
-
-#include
-#include
-#include
-#include
-#include
-#include
-
-// #include "easylogging++.h"
-
-using namespace std::chrono_literals;
-
-namespace hnurm
-{
-template
-class DataBuffer
-{
-public:
- DataBuffer() = default;
-
- ~DataBuffer() = default;
-
- bool Get(DataType &data);
-
- bool Update(const DataType &data);
-
-private:
- DataType data_buf;
- std::timed_mutex mtx;
- bool updated_flag = false;
-}; // class DataBuffer
-
-template
-bool DataBuffer::Get(DataType &data)
-{
- if(mtx.try_lock_for(2ms))
- {
- if(!updated_flag)
- {
- mtx.unlock();
- return false;
- }
- // LOG(WARNING) << "Success to lock in GET, UPDATED";
- data = data_buf;
- updated_flag = false;
- mtx.unlock();
- return true;
- }
- // LOG(WARNING) << "Failed to lock in GET";
- return false;
-}
-
-template
-bool DataBuffer::Update(const DataType &data)
-{
- if(mtx.try_lock_for(2ms))
- {
- // LOG(WARNING) << "Success to lock in UPDATE";
- data_buf = data;
- updated_flag = true;
- mtx.unlock();
- return true;
- }
- // LOG(WARNING) << "Failed to lock in UPDATE";
- return false;
-}
-} // namespace hnurm
-
-#endif // DataBuffer_HPP
diff --git a/hnurm_armor/include/hnurm_armor/DataType.hpp b/hnurm_armor/include/hnurm_armor/DataType.hpp
old mode 100755
new mode 100644
index d7cd3643baba8a3e04306bba3443649403885ced..35a24da35fa66b078114f9caddd0b3ce6cb3a210
--- a/hnurm_armor/include/hnurm_armor/DataType.hpp
+++ b/hnurm_armor/include/hnurm_armor/DataType.hpp
@@ -157,7 +157,8 @@ enum class ArmorsNum
{
NORMAL_4 = 4,
BALANCE_2 = 2,
- OUTPOST_3 = 3
+ OUTPOST_3 = 3,
+ BASE_1 = 1
};
} // namespace hnurm
diff --git a/hnurm_armor/include/hnurm_armor/Processor.h b/hnurm_armor/include/hnurm_armor/Processor.h
index 47c217d238692f7383169d5a17031a5fd88a2bbe..32f9681ef523b9357465c58e908f7805302183bd 100755
--- a/hnurm_armor/include/hnurm_armor/Processor.h
+++ b/hnurm_armor/include/hnurm_armor/Processor.h
@@ -5,8 +5,9 @@
#include "hnurm_armor/TSolver.hpp"
#include "hnurm_armor/armor.hpp"
#include "hnurm_armor/tracker.hpp"
+#include
+
#include
-#include
#include
#include
@@ -39,7 +40,7 @@ public:
*/
// void ProcessArmor(std::vector &armors_msg, VisionRecvData &recv_data, TargetInfo &target_msg);
void
- ProcessArmor(std::vector &armors_msg, hnurm_uart::msg::VisionRecvData &recv_data, TargetInfo &target_msg);
+ ProcessArmor(std::vector &armors_msg, hnurm_interfaces::msg::VisionRecvData &recv_data, TargetInfo &target_msg);
std::unique_ptr tracker;
std::unique_ptr tsolver;
diff --git a/hnurm_armor/include/hnurm_armor/TSolver.hpp b/hnurm_armor/include/hnurm_armor/TSolver.hpp
index 0a857c13500f6f687812dfbbd43bbafb711c866a..09a5150d27851377f426b22e47c049ebbcce146f 100755
--- a/hnurm_armor/include/hnurm_armor/TSolver.hpp
+++ b/hnurm_armor/include/hnurm_armor/TSolver.hpp
@@ -9,11 +9,9 @@
#include "hnurm_armor/armor.hpp"
#include "hnurm_armor/tracker.hpp"
-#include "solvePnPBA/solvePnPBA.h"
-
#include
#include
-
+#include
#include
#include
@@ -28,504 +26,279 @@
namespace hnurm
{
-class TSolver
-{
-public:
- explicit TSolver(const rclcpp::Node::SharedPtr &node)
+ class TSolver
{
- small_armor_points_ = {
- cv::Point3f(-SMALL_ARMOR_WIDTH / 2, 0, SMALL_ARMOR_HEIGHT / 2),
- cv::Point3f(-SMALL_ARMOR_WIDTH / 2, 0, -SMALL_ARMOR_HEIGHT / 2),
- cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, -SMALL_ARMOR_HEIGHT / 2),
- cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, SMALL_ARMOR_HEIGHT / 2),
-
- cv::Point3f(0 , 0 , SMALL_ARMOR_HEIGHT / 2),
- cv::Point3f(-SMALL_ARMOR_WIDTH / 2 , 0 , 0),
- cv::Point3f(0 , 0 , -SMALL_ARMOR_HEIGHT / 2),
- cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, 0),
-
- cv::Point3f(0, 0, 0),
-
- };
-
-
- large_armor_points_ = {
- cv::Point3f(-LARGE_ARMOR_WIDTH / 2, 0, LARGE_ARMOR_HEIGHT / 2),
- cv::Point3f(-LARGE_ARMOR_WIDTH / 2, 0, -LARGE_ARMOR_HEIGHT / 2),
- cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, -LARGE_ARMOR_HEIGHT / 2),
- cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, LARGE_ARMOR_HEIGHT / 2),
-
- cv::Point3f(0 , 0 , LARGE_ARMOR_WIDTH / 2),
- cv::Point3f(-LARGE_ARMOR_WIDTH / 2 , 0 , 0),
- cv::Point3f(0 , 0 , -LARGE_ARMOR_WIDTH / 2),
- cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, 0),
-
- cv::Point3f(0, 0, 0),
-
- };
-
- float cam_bias_z, cam_bias_y;
- cam_bias_z = node->declare_parameter("processor.solver.camera_bias_z", 0);
- cam_bias_y = node->declare_parameter("processor.solver.camera_bias_y", 0);
- _cam_bias << cam_bias_z, cam_bias_y, 0;
- std::string calib_info_path;
- // cfg_node["calib_info_path"] >> calib_info_path;
- calib_info_path = node->declare_parameter("processor.solver.calib_info_path", "src/hnurm_armor/data/");
- std::string camera_id;
- camera_id = node->declare_parameter("processor.solver.camera_id", "0");
- calib_info_path = calib_info_path + camera_id + ".yaml";
- cv::FileStorage calib_info(calib_info_path, cv::FileStorage::READ);
- if(!calib_info.isOpened())
+ public:
+ explicit TSolver(const rclcpp::Node::SharedPtr &node)
{
- RCLCPP_WARN_STREAM(node->get_logger(), "Failed to load calib info in " << calib_info_path);
+ small_armor_points_ = {
+ cv::Point3f(0, 0, SMALL_ARMOR_HEIGHT / 2),
+ cv::Point3f(-SMALL_ARMOR_WIDTH / 2, 0, 0),
+ cv::Point3f(0, 0, -SMALL_ARMOR_HEIGHT / 2),
+ cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, 0),
+ cv::Point3f(0, 0, 0),
+ };
+ small_armor_9points_ = {
+ cv::Point3f(-SMALL_ARMOR_WIDTH / 2, 0, SMALL_ARMOR_HEIGHT / 2),
+ cv::Point3f(-SMALL_ARMOR_WIDTH / 2, 0, -SMALL_ARMOR_HEIGHT / 2),
+ cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, -SMALL_ARMOR_HEIGHT / 2),
+ cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, SMALL_ARMOR_HEIGHT / 2),
+ cv::Point3f(0, 0, SMALL_ARMOR_HEIGHT / 2),
+ cv::Point3f(-SMALL_ARMOR_WIDTH / 2, 0, 0),
+ cv::Point3f(0, 0, -SMALL_ARMOR_HEIGHT / 2),
+ cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, 0),
+ cv::Point3f(0, 0, 0),
+ };
+ large_armor_points_ = {
+ cv::Point3f(0, 0, LARGE_ARMOR_WIDTH / 2),
+ cv::Point3f(-LARGE_ARMOR_WIDTH / 2, 0, 0),
+ cv::Point3f(0, 0, -LARGE_ARMOR_WIDTH / 2),
+ cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, 0),
+ cv::Point3f(0, 0, 0),
+ };
+ large_armor_9points_ = {
+ cv::Point3f(-LARGE_ARMOR_WIDTH / 2, 0, LARGE_ARMOR_HEIGHT / 2),
+ cv::Point3f(-LARGE_ARMOR_WIDTH / 2, 0, -LARGE_ARMOR_HEIGHT / 2),
+ cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, -LARGE_ARMOR_HEIGHT / 2),
+ cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, LARGE_ARMOR_HEIGHT / 2),
+ cv::Point3f(0, 0, LARGE_ARMOR_WIDTH / 2),
+ cv::Point3f(-LARGE_ARMOR_WIDTH / 2, 0, 0),
+ cv::Point3f(0, 0, -LARGE_ARMOR_WIDTH / 2),
+ cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, 0),
+ cv::Point3f(0, 0, 0),
+ };
+ float cam_bias_z, cam_bias_y;
+ cam_bias_z = node->declare_parameter("processor.solver.camera_bias_z", 0);
+ cam_bias_y = node->declare_parameter("processor.solver.camera_bias_y", 0);
+ _cam_bias << cam_bias_z, cam_bias_y, 0;
+ std::string calib_info_path;
+ // cfg_node["calib_info_path"] >> calib_info_path;
+ calib_info_path = node->declare_parameter("processor.solver.calib_info_path", "src/hnurm_armor/data/");
+ std::string camera_id;
+ camera_id = node->declare_parameter("processor.solver.camera_id", "0");
+ calib_info_path = calib_info_path + camera_id + ".yaml";
+ cv::FileStorage calib_info(calib_info_path, cv::FileStorage::READ);
+ if (!calib_info.isOpened())
+ {
+ RCLCPP_WARN_STREAM(node->get_logger(), "Failed to load calib info in " << calib_info_path);
+ }
+ calib_info["camera_matrix"] >> camera_matrix_;
+ calib_info["distortion_coefficients"] >> dist_coeffs_;
}
- calib_info["camera_matrix"] >> camera_matrix_;
- calib_info["distortion_coefficients"] >> dist_coeffs_;
- }
-
- // Get 3d position from 2d points
- bool GetTransformation(Armor &armor, float g_pitch, float g_yaw, const geometry_msgs::msg::TransformStamped&imu_to_gimbal_trans
- )
- {
- // 求解PnP问题
- // g_pitch = 0;
- // g_yaw=0;
- cv::Mat rvec, tvec;
- if(armor.armor_type == ArmorType::SMALL)
+ float distance(const cv::Point2f &p1, const cv::Point2f &p2)
{
- cv::solvePnP(
- small_armor_points_, armor.points2d, camera_matrix_, dist_coeffs_, rvec, tvec, false, cv::SOLVEPNP_IPPE
- );
-// solvePnPBA(small_armor_points_, armor.points2d, camera_matrix_, dist_coeffs_, rvec, tvec, true);
+ float dx = p1.x - p2.x;
+ float dy = p1.y - p2.y;
+ return sqrt(dx * dx + dy * dy);
}
- else if(armor.armor_type == ArmorType::LARGE)
- cv::solvePnP(
- large_armor_points_, armor.points2d, camera_matrix_, dist_coeffs_, rvec, tvec, false, cv::SOLVEPNP_IPPE
- );
-
- // 构造变换矩阵
- armor._translation << tvec.at(0, 0), tvec.at(1, 0), tvec.at(2, 0);
- Eigen::AngleAxisd rot_angle(
- cv::norm(rvec), Eigen::Vector3d(rvec.at(0, 0), rvec.at(1, 0), rvec.at(2, 0))
- );
- armor._rmat = rot_angle.matrix();
- float sp = std::sin(g_pitch), cp = std::cos(g_pitch), sy = std::sin(g_yaw), cy = std::cos(g_yaw);
- armor._r1 << 1, 0, 0,
- 0, 0, 1,
- 0, -1, 0;
- armor._r2 << 1, 0, 0,
- 0, cp, -sp,
- 0, sp, cp;
- armor._r3 << cy, -sy, 0,
- sy, cy, 0,
- 0, 0, 1;
- // 计算装甲板中心点在世界坐标系下的坐标,以及装甲板的旋转矩阵
-
- Eigen::Quaterniond cam_to_imu(1,0,0,0);
- Eigen::Quaterniond imu_to_gimbal;
- tf2::fromMsg(imu_to_gimbal_trans.transform.rotation, imu_to_gimbal);
-
- auto rot = imu_to_gimbal * cam_to_imu * armor._rmat;
- auto pos = (imu_to_gimbal * cam_to_imu * armor._translation) / 1'000;
- RCLCPP_INFO_STREAM(rclcpp::get_logger("TSolver"), "pos: " << pos.transpose());
- RCLCPP_INFO_STREAM(rclcpp::get_logger("TSolver"), "rot: " << rot.matrix().transpose());
-
- armor.rotation = armor._r3 * armor._r2 * armor._r1 * armor._rmat;
- armor.position = armor._r3 * armor._r2 * (armor._r1 * armor._translation) / 1'000;
-
- RCLCPP_INFO_STREAM(rclcpp::get_logger("TSolver"), "pos: " << armor.position.transpose());
- RCLCPP_INFO_STREAM(rclcpp::get_logger("TSolver"), "rot: " << armor.rotation.transpose());
- return true;
- }
-
- /**
- * @brief 实现可视化
- * @param curr_tracker
- * @param radius_1 读取到的装甲板和其相对的装甲板的距离
- * @param radius_2
- * @return
- */
- void ArmorVisualization(const Tracker &curr_tracker, const TargetInfo &target)
- {
- cv::Mat visualization(1'000, 1'000, CV_8UC3, cv::Scalar(0, 0, 0)); // 可视化界面
- cv::putText(
- visualization,
- "radius_1:" + std::to_string(target.radius_1),
- cv::Point(100, 100),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示距离
- cv::putText(
- visualization,
- "radius_2:" + std::to_string(target.radius_2),
- cv::Point(100, 150),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示距离
- cv::putText(
- visualization,
- "yaw" + std::to_string(angles::to_degrees(target.yaw)),
- cv::Point(100, 200),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- );
- cv::putText(
- visualization,
- "vx: " + std::to_string(target.vx),
- cv::Point(100, 400),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示w_yaw
- cv::putText(
- visualization,
- "x: " + std::to_string(target.x),
- cv::Point(100, 350),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示w_yaw
- cv::putText(
- visualization,
- "vy: " + std::to_string(target.vy),
- cv::Point(100, 500),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示w_yaw
- cv::putText(
- visualization,
- "y: " + std::to_string(target.y),
- cv::Point(100, 450),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示w_yaw
- cv::putText(
- visualization,
- "vz: " + std::to_string(target.vz),
- cv::Point(100, 600),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示w_yaw
- cv::putText(
- visualization,
- "z: " + std::to_string(target.z),
- cv::Point(100, 550),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示w_yaw
- cv::putText(
- visualization,
- "w_yaw: " + std::to_string(target.w_yaw),
- cv::Point(100, 250),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- ); // 显示w_yaw
-
- // write the tracker state
- std::string tracker_state;
- switch(curr_tracker.tracker_state)
+ float totalDistance(const std::vector &points1, const std::vector &points2)
+ {
+ float total_dist = 0.0f;
+ for (size_t i = 0; i < points1.size(); ++i)
+ {
+ total_dist += distance(points1[i], points2[i]);
+ }
+ return total_dist;
+ }
+ static void rotationMatrixToEulerAngles(const Eigen::Matrix3d &R, Eigen::Vector3d &eulerAngles)
{
- case Tracker::TRACKING:
- tracker_state = "TRACKING";
- break;
- case Tracker::LOST:
- tracker_state = "LOST";
- break;
- case Tracker::DETECTING:
- tracker_state = "DETECTING";
- break;
- case Tracker::TEMP_LOST:
- tracker_state = "TEMP_LOST";
- break;
+ double sy = sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0));
+ bool singular = sy < 1e-6; // If
+ if (!singular)
+ {
+ eulerAngles[0] = atan2(R(2, 1), R(2, 2)); // pitch
+ eulerAngles[1] = atan2(-R(2, 0), sy); // roll
+ eulerAngles[2] = atan2(R(1, 0), R(0, 0)); // yaw
+ }
+ else
+ {
+ eulerAngles[0] = atan2(-R(1, 2), R(1, 1));
+ eulerAngles[1] = atan2(-R(2, 0), sy);
+ eulerAngles[2] = 0;
+ }
}
- cv::putText(
- visualization,
- "tracker state:" + tracker_state,
- cv::Point(100, 300),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- );
- cv::putText(
- visualization,
- "tracker_armor_sum" + std::to_string((int)curr_tracker.tracked_armors_num),
- cv::Point(100, 325),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- );
- float bili = 0.1;
- float radius_1 = 2 * target.radius_1 * 1'000 * bili;
- float radius_2 = 2 * target.radius_2 * 1'000 * bili;
- float t_x = target.x * 1'000 * bili;
- float t_y = target.y * 1'000 * bili;
- cv::arrowedLine(
- visualization, cv::Point(0, 500), cv::Point(1'000, 500), cv::Scalar(255, 0, 0), 1, 8, 0, 0.01
- ); // 世界坐标系x轴 蓝色
- cv::arrowedLine(
- visualization, cv::Point(500, 1'000), cv::Point(500, 0), cv::Scalar(0, 255, 255), 1, 8, 0, 0.01
- ); // 世界坐标系y轴 黄色
- Eigen::Vector3d x_c(50 / bili, 0, 0);
- Eigen::Vector3d y_c(0, 50 / bili, 0);
- const auto armor = curr_tracker.tracked_armor;
- cv::putText(
- visualization,
- "xa: " + std::to_string(armor.position(0)),
- cv::Point(100, 700),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- );
- cv::putText(
- visualization,
- "ya: " + std::to_string(armor.position(1)),
- cv::Point(100, 750),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- );
- cv::putText(
- visualization,
- "za: " + std::to_string(armor.position(2)),
- cv::Point(100, 800),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- );
- double dis_armor_to_origin = armor.position.norm();
- double dis_center_to_origin = sqrt(pow(target.x, 2) + pow(target.y, 2) + pow(target.z, 2));
- cv::putText(
- visualization,
- "armor_dis: " + std::to_string(dis_armor_to_origin),
- cv::Point(800, 50),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- );
- cv::putText(
- visualization,
- "center_dis: " + std::to_string(dis_center_to_origin),
- cv::Point(800, 100),
- cv::FONT_HERSHEY_SIMPLEX,
- 0.5,
- cv::Scalar(255, 255, 255),
- 1,
- 8,
- false
- );
- Eigen::Vector3d x_w
- = armor._r3 * armor._r2 * (armor._r1 * (armor._rmat * x_c + armor._translation) + _cam_bias);
- Eigen::Vector3d z_w
- = armor._r3 * armor._r2 * (armor._r1 * (armor._rmat * y_c + armor._translation) + _cam_bias);
- Eigen::Vector3d center_w = armor._r3 * armor._r2 * (armor._r1 * armor._translation + _cam_bias);
- // std::cout<<"z_w-center_w"< armors;
- armors.push_back(armor1);
- armors.push_back(armor2);
- armors.push_back(armor3);
- armors.push_back(armor4);
- sort(armors.begin(), armors.end(), Compensator::cmp);
- double rad0 = atan2(armors[0].y, armors[0].x);
- double rad1 = atan2(armors[1].y, armors[1].x);
- double rad = atan2(target.y, target.x);
- if(abs(angles::shortest_angular_distance(rad0, rad)) < abs(angles::shortest_angular_distance(rad1, rad)))
+ float get_distance(const float &yaw_reg, Armor &armor, cv::Mat &tvec)
{
- fx = armors[0].x;
- fy = armors[0].y;
- fz = armors[0].z;
+ double pitch_deg;
+ if (armor.number == "outpost")
+ {
+ pitch_deg = 15.0;
+ }
+ else if (armor.number == "base")
+ {
+ pitch_deg = -62.5;
+ }
+ else
+ {
+ pitch_deg = -15.0;
+ }
+ double pitch_rad = pitch_deg * CV_PI / 180.0;
+ double yaw_rad = yaw_reg * CV_PI / 180.0;
+ Eigen::Matrix3d rotation_matrix_pitch_roll;
+ rotation_matrix_pitch_roll << 1, 0, 0,
+ 0, cos(pitch_rad), -sin(pitch_rad),
+ 0, sin(pitch_rad), cos(pitch_rad);
+ Eigen::Matrix3d rotation_matrix_yaw;
+ rotation_matrix_yaw << cos(yaw_rad), -sin(yaw_rad), 0,
+ sin(yaw_rad), cos(yaw_rad), 0,
+ 0, 0, 1;
+ Eigen::Matrix3d final_rotation_matrix = rotation_matrix_yaw * rotation_matrix_pitch_roll;
+ std::vector image_points;
+ cv::Matx33d final_rotation_matrix_opencv(
+ final_rotation_matrix(0, 0), final_rotation_matrix(0, 1), final_rotation_matrix(0, 2),
+ final_rotation_matrix(1, 0), final_rotation_matrix(1, 1), final_rotation_matrix(1, 2),
+ final_rotation_matrix(2, 0), final_rotation_matrix(2, 1), final_rotation_matrix(2, 2)
+ );
+ cv::Matx33d r;
+ cv::eigen2cv(armor._R, r);
+ if (armor.armor_type == ArmorType::SMALL)
+ {
+ projectPoints(small_armor_9points_,
+ r * final_rotation_matrix_opencv,
+ tvec,
+ camera_matrix_,
+ dist_coeffs_,
+ image_points);
+ }
+ else
+ {
+ projectPoints(large_armor_9points_,
+ r * final_rotation_matrix_opencv,
+ tvec,
+ camera_matrix_,
+ dist_coeffs_,
+ image_points);
+ }
+// std::vector last_five_points(armor.points2d.end() - 5, armor.points2d.end());
+ float distance_9 = totalDistance(armor.points2d, image_points);
+ return distance_9;
}
- else
+ // Get 3d position from 2d points
+ bool GetTransformation(Armor &armor, float g_pitch, float g_yaw)
{
- fx = armors[1].x;
- fy = armors[1].y;
- fz = armors[1].z;
+ // 求解PnP问题
+ // g_pitch = 0;
+ // g_yaw=0;
+ cv::Mat rvec, tvec;
+ if (armor.armor_type == ArmorType::SMALL)
+ {
+ cv::solvePnP(
+ small_armor_9points_,
+ armor.points2d,
+ camera_matrix_,
+ dist_coeffs_,
+ rvec,
+ tvec,
+ false,
+ cv::SOLVEPNP_IPPE
+ );
+// solvePnPBA(small_armor_points_, armor.points2d, camera_matrix_, dist_coeffs_, rvec, tvec, true);
+ }
+ else if (armor.armor_type == ArmorType::LARGE)
+ cv::solvePnP(
+ large_armor_9points_,
+ armor.points2d,
+ camera_matrix_,
+ dist_coeffs_,
+ rvec,
+ tvec,
+ false,
+ cv::SOLVEPNP_IPPE
+ );
+
+ // 构造变换矩阵
+ armor._translation << tvec.at(0, 0), tvec.at(1, 0), tvec.at(2, 0);
+ Eigen::AngleAxisd rot_angle(
+ cv::norm(rvec), Eigen::Vector3d(rvec.at(0, 0), rvec.at(1, 0), rvec.at(2, 0))
+ );
+ armor._rmat = rot_angle.matrix();
+ float sp = std::sin(g_pitch), cp = std::cos(g_pitch), sy = std::sin(g_yaw), cy = std::cos(g_yaw);
+ armor._r1 << 1, 0, 0,
+ 0, 0, 1,
+ 0, -1, 0;
+ armor._r2 << 1, 0, 0,
+ 0, cp, -sp,
+ 0, sp, cp;
+ armor._r3 << cy, -sy, 0,
+ sy, cy, 0,
+ 0, 0, 1;
+ armor._R = (armor._r3 * armor._r2 * armor._r1).inverse();
+ armor.rotation = armor._r3 * armor._r2 * armor._r1 * armor._rmat;
+ armor.position = armor._r3 * armor._r2 * (armor._r1 * armor._translation) / 1'000;
+ Eigen::Vector3d eulerAngles;
+ rotationMatrixToEulerAngles(armor.rotation, eulerAngles);
+ float a = eulerAngles[2] * 180 / CV_PI - 35; // 搜索区间起点
+ float b = eulerAngles[2] * 180 / CV_PI + 35; // 搜索区间终点
+ float tolerance = 0.1; // 精度阈值
+ const float phi = (1 + std::sqrt(5)) / 2; // 黄金分割比
+ float c = b - (b - a) / phi;
+ float d = a + (b - a) / phi;
+ while (std::fabs(b - a) > tolerance)
+ {
+ float s1 = get_distance(c, armor, tvec);
+ float s2 = get_distance(d, armor, tvec);
+ if (s1 < s2)
+ {
+ b = d;
+ }
+ else
+ {
+ a = c;
+ }
+ c = b - (b - a) / phi;
+ d = a + (b - a) / phi;
+ }
+ float right_yaw = (a + b) / 2 * CV_PI / 180;
+ Eigen::Matrix3d rotation_matrix_pitch_roll;
+ double pitch_deg;
+ if (armor.number == "outpost")
+ {
+ pitch_deg = 15.0;
+ }
+ else if (armor.number == "base")
+ {
+ pitch_deg = -62.5;
+ }
+ else
+ {
+ pitch_deg = -15.0;
+ }
+ rotation_matrix_pitch_roll << 1, 0, 0,
+ 0, cos(pitch_deg * CV_PI / 180.0), -sin(pitch_deg * CV_PI / 180.0),
+ 0, sin(pitch_deg * CV_PI / 180.0), cos(pitch_deg * CV_PI / 180.0);
+ Eigen::Matrix3d rotation_matrix_yaw;
+ rotation_matrix_yaw << cos(right_yaw), -sin(right_yaw), 0,
+ sin(right_yaw), cos(right_yaw), 0,
+ 0, 0, 1;
+ armor.rotation = rotation_matrix_yaw * rotation_matrix_pitch_roll;
+ return true;
}
- circle(
- visualization,
- cv::Point((int)(500 + fx * 1'000 * bili), (int)(500 - fy * 1'000 * bili)),
- 3,
- cv::Scalar(255, 255, 255),
- -1
- );
- /// 111
- line(
- visualization,
- cv::Point(
- (int)(500 + t_x - radius_1 * cos(target.yaw) / 2), (int)(500 - t_y + radius_1 * sin(target.yaw) / 2)
- ),
- cv::Point(
- (int)(500 + t_x + radius_1 * cos(target.yaw) / 2), (int)(500 - t_y - radius_1 * sin(target.yaw) / 2)
- ),
- cv::Scalar(0, 0, 255),
- 2
- ); // 根据半径画出车
- line(
- visualization,
- cv::Point(
- (int)(500 + t_x - radius_2 * sin(target.yaw) / 2), (int)(500 - t_y - radius_2 * cos(target.yaw) / 2)
- ),
- cv::Point(
- (int)(500 + t_x + radius_2 * sin(target.yaw) / 2), (int)(500 - t_y + radius_2 * cos(target.yaw) / 2)
- ),
- cv::Scalar(0, 0, 255),
- 2
- ); // 根据半径画出车
- circle(visualization, cv::Point((int)(500 + t_x), (int)(500 - t_y)), 3, cv::Scalar(0, 0, 255), -1); // 车的中心
- cv::namedWindow("visualization", cv::WINDOW_AUTOSIZE);
- cv::imshow("visualization", visualization);
- cv::waitKey(1);
- }
-
-private:
- cv::Mat camera_matrix_;
- cv::Mat dist_coeffs_;
- // Unit: mm
- static constexpr float SMALL_ARMOR_WIDTH = 135;
- static constexpr float SMALL_ARMOR_HEIGHT = 55;
- static constexpr float LARGE_ARMOR_WIDTH = 225;
- static constexpr float LARGE_ARMOR_HEIGHT = 55;
- // Four vertices of armor in 3d
- std::vector small_armor_points_;
- std::vector large_armor_points_;
- Eigen::Vector3d _cam_bias;
- double angle = 0.2 * CV_PI;
- float lastr = 0.14;
- float radius = 0.24;
- cv::Point2f ccc = cv::Point2f(3, 3);
- std::vector armorss;
- std::vector armorss1;
-};
+ private:
+ cv::Mat camera_matrix_;
+ cv::Mat dist_coeffs_;
+
+ static constexpr float SMALL_ARMOR_WIDTH = 131.25;
+ static constexpr float SMALL_ARMOR_HEIGHT = 55.5;
+ static constexpr float LARGE_ARMOR_WIDTH = 226.25;
+ static constexpr float LARGE_ARMOR_HEIGHT = 55.5;
+
+ // Four vertices of armor in 3d
+ std::vector small_armor_points_;
+ std::vector small_armor_9points_;
+ std::vector large_armor_points_;
+ std::vector large_armor_9points_;
+ Eigen::Vector3d _cam_bias;
+ double angle = 0.2 * CV_PI;
+ float lastr = 0.14;
+ float radius = 0.24;
+ cv::Point2f ccc = cv::Point2f(3, 3);
+ std::vector armorss;
+ std::vector armorss1;
+ };
} // namespace hnurm
diff --git a/hnurm_armor/include/hnurm_armor/armor.hpp b/hnurm_armor/include/hnurm_armor/armor.hpp
index cfa01e5afc142f5069172f70bef464eccc4864de..bb35ed3991052e6e0a5e53b0eaf0e06181c16490 100755
--- a/hnurm_armor/include/hnurm_armor/armor.hpp
+++ b/hnurm_armor/include/hnurm_armor/armor.hpp
@@ -29,7 +29,7 @@ struct Light : public cv::RotatedRect
explicit Light(const cv::RotatedRect &box) : cv::RotatedRect(box)
{
- cv::Point2f p[4];
+
box.points(p);
std::sort(p, p + 4, [](const cv::Point2f &a, const cv::Point2f &b) { return a.y < b.y; });
top = (p[0] + p[1]) / 2;
@@ -47,6 +47,7 @@ struct Light : public cv::RotatedRect
double length;
double width;
float tilt_angle;
+ cv::Point2f p[4];
};
struct Armor
@@ -112,6 +113,7 @@ struct Armor
Eigen::Matrix3d _r1; // 相机坐标系到云台坐标系
Eigen::Matrix3d _r2; // 先对pitch进行旋转
Eigen::Matrix3d _r3; // 再对yaw的旋转,变为世界坐标系(pitch、yaw为0)
+ Eigen::Matrix3d _R; //本体系到相机系
Eigen::Matrix3d _rmat;
Eigen::Vector3d _translation;
};
diff --git a/hnurm_armor/include/hnurm_armor/armor_node.hpp b/hnurm_armor/include/hnurm_armor/armor_node.hpp
index 218922174365923a86b57caad6fcfa93877de74a..9a55a27fdc1717f7e7bfe4d2dc9aacdb89e6315e 100644
--- a/hnurm_armor/include/hnurm_armor/armor_node.hpp
+++ b/hnurm_armor/include/hnurm_armor/armor_node.hpp
@@ -2,30 +2,39 @@
#include "hnurm_armor/Compensator.h"
#include "hnurm_armor/Processor.h"
-#include "hnurm_armor/msg/target.hpp"
-#include "hnurm_armor/msg/tracker_info.hpp"
-#include "hnurm_detect/msg/armor_array.hpp"
+#include "hnurm_interfaces/msg/armor_array.hpp"
+#include "hnurm_interfaces/msg/target.hpp"
+#include "hnurm_interfaces/msg/tracker_info.hpp"
#include
+#include "hnurm_armor/armor.hpp"
+#include
+#include
+#include
+
namespace hnurm
{
-
-std::vector fromROSMsg(const hnurm_detect::msg::ArmorArray &armors);
+std::vector fromROSMsg(const hnurm_interfaces::msg::ArmorArray &armors);
class ArmorNode : public rclcpp::Node
{
public:
explicit ArmorNode(const rclcpp::NodeOptions &options);
- ~ArmorNode() {RCLCPP_INFO(get_logger(), "ArmorNode destroyed");}
+ ~ArmorNode()
+ {
+ RCLCPP_INFO(get_logger(), "ArmorNode destroyed");
+ }
void init_markers();
- void send_target(TargetInfo &target, hnurm_uart::msg::VisionRecvData &uart_msg);
+ void send_target(TargetInfo &target, hnurm_interfaces::msg::VisionRecvData &uart_msg);
+
+ void publish_markers(hnurm_interfaces::msg::Target &target_msg);
- void publish_markers(hnurm_armor::msg::Target &target_msg);
+ void timer_callback();
ArmorNode(const ArmorNode &) = delete;
ArmorNode &operator=(const ArmorNode &) = delete;
@@ -39,15 +48,17 @@ protected:
}
private:
- void armor_array_callback(hnurm_detect::msg::ArmorArray::SharedPtr msg);
+ void armor_array_callback(hnurm_interfaces::msg::ArmorArray::SharedPtr msg);
private:
- rclcpp::Publisher::SharedPtr send_data_pub_;
- rclcpp::Subscription::SharedPtr armor_array_sub_;
+ rclcpp::Publisher::SharedPtr send_data_pub_;
+ rclcpp::Subscription::SharedPtr armor_array_sub_;
rclcpp::Publisher::SharedPtr marker_pub_;
- rclcpp::Publisher::SharedPtr target_pub_;
- rclcpp::Publisher::SharedPtr tracker_info_pub_;
+ rclcpp::Publisher::SharedPtr target_pub_;
+ rclcpp::Publisher::SharedPtr tracker_info_pub_;
+
+ rclcpp::TimerBase::SharedPtr timer_;
// markers
visualization_msgs::msg::Marker position_marker_;
@@ -57,5 +68,14 @@ private:
std::unique_ptr processor_;
std::unique_ptr compensator_;
+ std::vector armors;
+ std::mutex mtx;
+
+ hnurm_interfaces::msg::ArmorArray last_recv_array_;
+ std::vector pv;
+
+ float calculateAverage(const std::vector &vec);
+
+ void Averagedistance(std::vector &p, float value);
};
}
\ No newline at end of file
diff --git a/hnurm_armor/include/hnurm_armor/tracker.hpp b/hnurm_armor/include/hnurm_armor/tracker.hpp
index 55cb18e6e65691a88319c1e52d6225e801a54802..629bdca00ec66ef9c128b1b42738235771997bb2 100755
--- a/hnurm_armor/include/hnurm_armor/tracker.hpp
+++ b/hnurm_armor/include/hnurm_armor/tracker.hpp
@@ -32,6 +32,7 @@ public:
DETECTING,
TRACKING,
TEMP_LOST,
+ JUMP,
} tracker_state;
ExtendedKalmanFilter ekf;
@@ -49,7 +50,8 @@ public:
// To store another pair of armors message
double dz {}, another_r {};
static int i_jump;
-
+ double armor_pitch{};
+ double armor_yaw{};
// todo
double orientationToYaw(const Eigen::Matrix3d &R);
@@ -72,7 +74,7 @@ private:
int lost_count_ {};
double last_yaw_ {};
- double armor_pitch {};
+
};
} // namespace hnurm
diff --git a/hnurm_armor/include/solvePnPBA/solvePnPBA.h b/hnurm_armor/include/solvePnPBA/solvePnPBA.h
deleted file mode 100644
index 62ecb985b5e5824199e353b0aef7d907546d5a2e..0000000000000000000000000000000000000000
--- a/hnurm_armor/include/solvePnPBA/solvePnPBA.h
+++ /dev/null
@@ -1,149 +0,0 @@
-//
-// Created by Kai Wang on 24-3-15.
-//
-
-#ifndef SCRIPTS_SOLVEPNP_BA_H
-#define SCRIPTS_SOLVEPNP_BA_H
-
-#include
-#include
-#include
-
-#include
-
-#include
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-
-#define TIMEIT(code) \
- do \
- { \
- std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); \
- code; \
- std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); \
- std::chrono::duration time_used = std::chrono::duration_cast>(t2 - t1); \
- std::cout << "Time cost: " << time_used.count() << " seconds." << std::endl; \
- } \
- while(0)
-
-using VecVector3d = std::vector>;
-using VecVector2d = std::vector>;
-
-class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>
-{
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-
- void setToOriginImpl() override
- {
- _estimate = Sophus::SE3d();
- }
-
- /// left multiplication on SE3
- void oplusImpl(const double *update) override
- {
- Eigen::Matrix update_eigen;
- update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
- _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
- }
-
- bool read(std::istream &in) override
- {
- return true;
- }
-
- bool write(std::ostream &out) const override
- {
- return true;
- }
-};
-
-class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>
-{
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-
- EdgeProjection(Eigen::Vector3d pos, Eigen::Matrix3d K) : pose_3d_(std::move(pos)), K_(std::move(K))
- {
- }
-
- void computeError() override
- {
- const VertexPose *v = dynamic_cast(_vertices[0]);
- Sophus::SE3d T = v->estimate();
- Eigen::Vector3d pos_pixel = K_ * (T * pose_3d_);
- pos_pixel /= pos_pixel[2];
- _error = _measurement - pos_pixel.head<2>();
- }
-
- void linearizeOplus() override
- {
- const VertexPose *v = dynamic_cast(_vertices[0]);
- Sophus::SE3d T = v->estimate();
- Eigen::Vector3d pos_cam = T * pose_3d_;
- double fx = K_(0, 0);
- double fy = K_(1, 1);
- double cx = K_(0, 2);
- double cy = K_(1, 2);
- double X = pos_cam[0];
- double Y = pos_cam[1];
- double Z = pos_cam[2];
- double Z2 = Z * Z;
- _jacobianOplusXi << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z, 0, -fy / Z,
- fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
- }
-
- bool read(std::istream &in) override
- {
- return true;
- }
-
- bool write(std::ostream &out) const override
- {
- return true;
- }
-
-private:
- Eigen::Vector3d pose_3d_;
- Eigen::Matrix3d K_;
-};
-
-/**
- * @brief do bundle adjustment with g2o
- * @param points_3d vector of 3d points, i.e. object points in world coordinate
- * @param points_2d vector of 2d points, i.e. image points in pixel coordinate
- * @param K camera intrinsic matrix
- * @param pose pose of camera, initial value is used as input, and refined pose is output
- */
-void bundleAdjustmentG2O(
- const VecVector3d &points_3d, const VecVector2d &points_2d, const cv::Mat &K, Sophus::SE3d &pose
-);
-
-/**
- * @brief solve PnP problem with bundle adjustment
- * @param object_points the 3d points in world coordinate
- * @param image_points the 2d points in pixel coordinate
- * @param cameraMatrix camera intrinsic matrix
- * @param distCoeffs distortion coefficients [not used]
- * @param rvec rotation vector
- * @param tvec translation vector
- * @param useExtrinsicGuess whether to use the initial value from rvec and tvec
- * @return
- */
-bool solvePnPBA(
- cv::InputArray object_points,
- cv::InputArray image_points,
- cv::InputArray cameraMatrix,
- cv::InputArray distCoeffs,
- cv::OutputArray rvec,
- cv::OutputArray tvec,
- bool useExtrinsicGuess
-);
-
-#endif // SCRIPTS_SOLVEPNP_BA_H
diff --git a/hnurm_armor/include/solvePnPBA/test.cpp b/hnurm_armor/include/solvePnPBA/test.cpp
deleted file mode 100644
index ea8e061390ded77977dac404512a2255edd92734..0000000000000000000000000000000000000000
--- a/hnurm_armor/include/solvePnPBA/test.cpp
+++ /dev/null
@@ -1,89 +0,0 @@
-//
-// Created by Kai Wang on 24-3-15.
-//
-
-#include "solvePnPBA.h"
-
-void data_generation(
- const Eigen::Isometry3d &transform, const Eigen::Matrix3d &K, VecVector3d &obj_points, VecVector2d &img_points
-)
-{
- obj_points.clear();
- img_points.clear();
- double z = 1;
- obj_points.emplace_back(-2, 1, z);
- obj_points.emplace_back(2, 1, z);
- obj_points.emplace_back(-2, -1, z);
- obj_points.emplace_back(2, -1, z);
-
- for(const auto &obj_point : obj_points)
- {
- Eigen::Vector3d cam_point = transform * obj_point;
- Eigen::Vector3d pixel = K * cam_point;
- pixel /= pixel[2];
- // add noise
- std::random_device rd;
- std::default_random_engine generator(rd());
- std::normal_distribution noise(0, 20);
- pixel[0] += noise(generator);
- pixel[1] += noise(generator);
- img_points.emplace_back(pixel[0], pixel[1]);
- }
-}
-
-int main()
-{
- // 相机内参
- Eigen::Matrix3d K;
- K << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
-
- // 相机位姿
- Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
- T.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(0, 0, 1)));
- T.rotate(Eigen::AngleAxisd(M_PI / 8, Eigen::Vector3d(0, 1, 0)));
- T.rotate(Eigen::AngleAxisd(M_PI / 10, Eigen::Vector3d(1, 0, 0)));
- T.pretranslate(Eigen::Vector3d(0.7, 0.8, 2.0));
-
- // 生成3D-2D观测数据
- VecVector3d obj_points;
- VecVector2d img_points;
- data_generation(T, K, obj_points, img_points);
-
- cv::Mat obj_mat(obj_points.size(), 3, CV_64F);
- cv::Mat img_mat(img_points.size(), 2, CV_64F);
- for(int i = 0; i < obj_points.size(); i++)
- {
- obj_mat.at(i, 0) = obj_points[i][0];
- obj_mat.at(i, 1) = obj_points[i][1];
- obj_mat.at(i, 2) = obj_points[i][2];
- img_mat.at(i, 0) = img_points[i][0];
- img_mat.at(i, 1) = img_points[i][1];
- }
-
- cv::Mat K_mat(3, 3, CV_64F);
- cv::eigen2cv(K, K_mat);
-
- // 调用g2o进行优化
- Sophus::SE3d pose_g2o;
- cv::Mat r, t;
-
- cv::Mat rvec, tvec;
- // opencv 的PNP求解
- TIMEIT(cv::solvePnP(obj_mat, img_mat, K_mat, cv::Mat(), rvec, tvec, false, cv::SOLVEPNP_IPPE););
-
- cv::Mat R;
- cv::Rodrigues(rvec, R);
- std::cout << "opencv estimated rvec =\n" << R << std::endl;
- std::cout << "opencv estimated tvec =\n" << tvec << std::endl;
-
- TIMEIT(solvePnPBA(obj_mat, img_mat, K_mat, cv::noArray(), r, t, false););
-
- cv::Rodrigues(r, R);
- std::cout << "ba estimated rvec =\n" << R << std::endl;
- std::cout << "ba estimated tvec =\n" << t << std::endl;
-
- // gt
- std::cout << "gt r =\n" << T.matrix() << std::endl;
-
- return 0;
-}
diff --git a/hnurm_armor/package.xml b/hnurm_armor/package.xml
index f399a00911cf3a699186f172011f225cff096a01..c4d1c0fea0de3e4e456e20dbc7711cb704e739a3 100644
--- a/hnurm_armor/package.xml
+++ b/hnurm_armor/package.xml
@@ -18,13 +18,15 @@
angles
visualization_msgs
+ tf2
+ tf2_ros
+ tf2_eigen
+ tf2_geometry_msgs
+
Eigen3
OpenCV
- Sophus
- g2o
- hnurm_uart
- hnurm_detect
+ hnurm_interfaces
rosidl_interface_packages
diff --git a/hnurm_armor/params/default.yaml b/hnurm_armor/params/default.yaml
index cdae9e169fae148dd1f81f00b3f7499643b35e95..0562d49aa6e6937038b924c5c781386907b29695 100644
--- a/hnurm_armor/params/default.yaml
+++ b/hnurm_armor/params/default.yaml
@@ -2,10 +2,12 @@
ros__parameters:
# topics
- vision_send_topic: "vision_send_res"
+ vision_send_topic: "vision_send_data"
tracker_info_topic: "tracker_info"
target_topic: "target"
+ using_timer: false
+
#####################################
# configs for compensator module #
@@ -13,22 +15,22 @@
compensator:
gravity: 9.8
friction_coeff: 0.02
- bullet_speed_bias: -0.5
- channel_delay: 0.01 # s
+ bullet_speed_bias: 0.0
+ channel_delay: 0.05 # s
yaw_bias: 0.0
pitch_bias: 0.0 # dirty param
- imutofrt_y: 0 #单位:米
- imutofrt_z: 0
+ imutofrt_y: 0.08418 #单位:米
+ imutofrt_z: 0.005
#####################################
# configs for processor module #
#####################################
processor:
tracker:
- max_match_distance: 0.5 # meter
+ max_match_distance: 1.0 # meter
max_match_yaw_diff: 0.5 # rads
tracking_threshold: 0.0 # frames
- lost_threshold: 15.0 # seconds
+ lost_threshold: 100.0 # seconds
# 过程噪声
s2_q_xyz: 0.05
@@ -37,11 +39,11 @@
# 测量噪声
r_xyz_factor: 4e-4
- r_yaw: 5e-3
+ r_yaw: 7e-3
solver:
- calib_info_path: "src/hnurm_armor/data/"
+ calib_info_path: "/home/rm/hnuvision/src/hnurm_armor/data/"
# bias to imu
- camera_bias_z: 0 # 单位:毫米
- camera_bias_y: 0
- camera_id: 2BDF89504499
+ camera_bias_z: 74 # 单位:毫米
+ camera_bias_y: 200
+ camera_id: 2BDFA2819639
diff --git a/hnurm_armor/src/Compensator.cpp b/hnurm_armor/src/Compensator.cpp
index fbb86216bbee08459ab15ce9ba19207c2dc3759d..f4895e9ee8423d272db53d2f91dce5f4c4cfac04 100755
--- a/hnurm_armor/src/Compensator.cpp
+++ b/hnurm_armor/src/Compensator.cpp
@@ -7,30 +7,55 @@
namespace hnurm
{
-Compensator::Compensator(rclcpp::Node::SharedPtr node) : node_(std::move(node))
-{
- gravity_ = static_cast(node_->declare_parameter("compensator.gravity", 9.8));
- friction_coeff_ = static_cast(node_->declare_parameter("compensator.friction_coeff", 0.02));
- bullet_speed_bias_ = static_cast(node_->declare_parameter("compensator.bullet_speed_bias", -0.5));
- channel_delay_ = static_cast(node_->declare_parameter("compensator.channel_delay", 0.01));
- yaw_bias_ = static_cast(node_->declare_parameter("compensator.yaw_bias", 0.0));
- pitch_bias_ = static_cast(node_->declare_parameter("compensator.pitch_bias", 0.0));
- imutofrt_y = static_cast(node_->declare_parameter("compensator.imutofrt_y", 0.0));
- imutofrt_z = static_cast(node_->declare_parameter("compensator.imutofrt_z", 0.0));
+ Compensator::Compensator(rclcpp::Node::SharedPtr node)
+ : node_(std::move(node))
+ {
+ gravity_ = static_cast(node_->declare_parameter("compensator.gravity", 9.8));
+ friction_coeff_ = static_cast(node_->declare_parameter("compensator.friction_coeff", 0.006));
+ bullet_speed_bias_ = static_cast(node_->declare_parameter("compensator.bullet_speed_bias", -0.5));
+ channel_delay_ = static_cast(node_->declare_parameter("compensator.channel_delay", 1.2));
+ yaw_bias_ = static_cast(node_->declare_parameter("compensator.yaw_bias", 0.0));
+ pitch_bias_ = static_cast(node_->declare_parameter("compensator.pitch_bias", 0.0));
+ imutofrt_y = static_cast(node_->declare_parameter("compensator.imutofrt_y", 0.0));
+ imutofrt_z = static_cast(node_->declare_parameter("compensator.imutofrt_z", 0.0));
- RCLCPP_INFO(node_->get_logger(), "Compensator initialized, yaw_bias: %f, pitch_bias: %f", yaw_bias_, pitch_bias_);
+ RCLCPP_INFO(node_->get_logger(),
+ "Compensator initialized, yaw_bias: %f, pitch_bias: %f",
+ yaw_bias_,
+ pitch_bias_);
- fx = fy = fz = 0;
- pout = yout = 0;
- tmp_fly_time = channel_delay_;
- bullet_speed_ = 0;
-}
+ fx = fy = fz = fv = yaw_difference = 0;
+ pout = yout = 0;
+ tmp_fly_time = channel_delay_;
+ bullet_speed_ = 0;
+ px = py = pz = pv = {};
+ }
-bool Compensator::cmp(const cv::Point3f &a, const cv::Point3f &b)
-{
- return pow(a.x, 2) + pow(a.y, 2) < pow(b.x, 2) + pow(b.y, 2);
-}
+ float Compensator::calculateAverage(const std::vector &vec)
+ {
+ int n = vec.size();
+ if (n == 0) return 0.1f; // 如果向量为空,返回0
+
+ float sum = 0.0f;
+ for (float value: vec)
+ {
+ sum += value;
+ }
+ return sum / n;
+ }
+ void Compensator::Averagedistance(std::vector &p, float value)
+ {
+ if (p.size() >= 50)
+ {
+ p.erase(p.begin());
+ }
+ p.push_back(value);
+ }
+ bool Compensator::cmp(const predict_armor &a, const predict_armor &b)
+ {
+ return a.yaw < b.yaw;
+ }
double Compensator::diff_eq(double v) const
{
return -gravity_ - (friction_coeff_ * abs(v) * v);
@@ -40,126 +65,197 @@ bool Compensator::cmp(const cv::Point3f &a, const cv::Point3f &b)
double v = v0;
double y = 0.0;
double dt = 0.001;
- for (double time = 0; time <= t; time += dt) {
+ for (double time = 0; time <= t; time += dt)
+ {
double dv = diff_eq(v);
v += dv * dt;
y += v * dt;
}
return y;
}
-float Compensator::CalcFinalAngle(
- TargetInfo &target_msg, hnurm_uart::msg::VisionRecvData &recv_data, hnurm_uart::msg::VisionSendData &send_data,ArmorsNum &armors_num
-)
-{
- tmp_fly_time = channel_delay_;
- // todo: fix this
- auto bullet_speed = recv_data.bullet_speed.data;
- auto g_yaw = recv_data.yaw;
- t_info = target_msg;
- bullet_speed_ = static_cast(bullet_speed) + bullet_speed_bias_;
- // 迭代三次
- for(int i = 0; i < 5; i++)
+ float Compensator::CalcFinalAngle(
+ TargetInfo &target_msg,
+ hnurm_interfaces::msg::VisionRecvData &recv_data,
+ hnurm_interfaces::msg::VisionSendData &send_data,
+ ArmorsNum &armors_num
+ )
{
- PredictPose(armors_num);
- CompensatePitch();
- }
- yout = (float) (g_yaw + angles::shortest_angular_distance(g_yaw * CV_PI / 180, -atan2(fx_center, fy_center)) * 180 / CV_PI);
- auto armor_yaw = (float) (g_yaw + angles::shortest_angular_distance(g_yaw * CV_PI / 180, -atan2(fx, fy)) * 180 / CV_PI);
- send_data.pitch = pout;
- send_data.yaw = yout;
- return armor_yaw;
-}
-
-// 根据飞行时间预测目标位置 todo
-void Compensator::PredictPose(ArmorsNum &armors_num)
-{
- if (abs(t_info.w_yaw) > 1 || (abs(t_info.vx) + abs(t_info.vy) + abs(t_info.vz)) > 1) {
- fx = t_info.x + tmp_fly_time * t_info.vx;// 根据飞行时间和速度计算预测的x坐标
- fy = t_info.y + tmp_fly_time * t_info.vy;// 根据飞行时间和速度计算预测的y坐标
- fz = t_info.z + tmp_fly_time * t_info.vz;// 根据飞行时间和速度计算预测的z坐标
- fx_center = fx;
- fy_center = fy;
- float yaw = t_info.yaw + tmp_fly_time * t_info.w_yaw;// 计算预测的偏航角
- cv::Point3f armor1, armor2, armor3, armor4;
- std::vector armor;
- if (armors_num == ArmorsNum::OUTPOST_3) {
- armor1.x = fx - t_info.radius_1 * cos(yaw); // 计算预测的护甲点1的x坐标
- armor1.y = fy - t_info.radius_1 * sin(yaw); // 计算预测的护甲点1的y坐标
- armor1.z = fz; // 设置预测的护甲点1的z坐标为当前的z坐标
- armor2.x = fx - t_info.radius_1 * cos(float(yaw + 2 * CV_PI / 3));// 计算预测的护甲点2的x坐标
- armor2.y = fy - t_info.radius_1 * sin(float(yaw + 2 * CV_PI / 3));// 计算预测的护甲点2的y坐标
- armor2.z = fz; // 设置预测的护甲点2的z坐标为当前的z坐标
- armor3.x = fx - t_info.radius_1 * sin(float(yaw + 4 * CV_PI / 3));// 计算预测的护甲点3的x坐标
- armor3.y = fy - t_info.radius_1 * cos(float(yaw + 4 * CV_PI / 3));// 计算预测的护甲点3的y坐标
- armor3.z = fz;
- armor.push_back(armor1);// 将预测的护甲点1加入vector
- armor.push_back(armor2);// 将预测的护甲点2加入vector
- armor.push_back(armor3);// 将预测的护甲点3加入vector
- } else if (armors_num == ArmorsNum::NORMAL_4) {
- armor1.x = fx - t_info.radius_1 * cos(yaw);// 计算预测的护甲点1的x坐标
- armor1.y = fy - t_info.radius_1 * sin(yaw);// 计算预测的护甲点1的y坐标
- armor1.z = fz; // 设置预测的护甲点1的z坐标为当前的z坐标
- armor2.x = fx + t_info.radius_1 * cos(yaw);// 计算预测的护甲点2的x坐标
- armor2.y = fy + t_info.radius_1 * sin(yaw);// 计算预测的护甲点2的y坐标
- armor2.z = fz; // 设置预测的护甲点2的z坐标为当前的z坐标
- armor3.x = fx - t_info.radius_2 * sin(yaw);// 计算预测的护甲点3的x坐标
- armor3.y = fy + t_info.radius_2 * cos(yaw);// 计算预测的护甲点3的y坐标
- armor3.z = fz + t_info.dz; // 设置预测的护甲点3的z坐标为当前的z坐标加上护甲的深度
- armor4.x = fx + t_info.radius_2 * sin(yaw);// 计算预测的护甲点4的x坐标
- armor4.y = fy - t_info.radius_2 * cos(yaw);// 计算预测的护甲点4的y坐标
- armor4.z = fz + t_info.dz; // 设置预测的护甲点4的z坐标为当前的z坐标加上护甲的深度
- armor.push_back(armor1); // 将预测的护甲点1加入vector
- armor.push_back(armor2); // 将预测的护甲点2加入vector
- armor.push_back(armor3); // 将预测的护甲点3加入vector
- armor.push_back(armor4); // 将预测的护甲点4加入vector
- } else if (armors_num == ArmorsNum::BALANCE_2) {
- armor1.x = fx - t_info.radius_1 * cos(yaw);// 计算预测的护甲点1的x坐标
- armor1.y = fy - t_info.radius_1 * sin(yaw);// 计算预测的护甲点1的y坐标
- armor1.z = fz; // 设置预测的护甲点1的z坐标为当前的z坐标
- armor2.x = fx + t_info.radius_1 * cos(yaw);// 计算预测的护甲点2的x坐标
- armor2.y = fy + t_info.radius_1 * sin(yaw);// 计算预测的护甲点2的y坐标
- armor2.z = fz; // 设置预测的护甲点2的z坐标为当前的z坐标
- armor.push_back(armor1); // 将预测的护甲点1加入vector
- armor.push_back(armor2); // 将预测的护甲点2加入vector
+ tmp_fly_time = channel_delay_;
+ // todo: fix this
+ auto bullet_speed = recv_data.bullet_speed.data;
+ auto g_yaw = recv_data.yaw;
+ t_info = target_msg;
+ bullet_speed_ = static_cast(bullet_speed) + bullet_speed_bias_;
+ if (armors_num == ArmorsNum::OUTPOST_3 || armors_num == ArmorsNum::BASE_1)
+ {
+ Averagedistance(px, t_info.x);
+ Averagedistance(py, t_info.y);
+ Averagedistance(pz, t_info.z);
+ Averagedistance(pv, t_info.w_yaw);
}
-
- sort(armor.begin(), armor.end(), cmp); // 根据护甲点的x,y坐标排序
- double rad0 = atan2(armor[0].y, armor[0].x);// 计算排序后的第一个护甲点的航向角
- double rad1 = atan2(armor[1].y, armor[1].x);
- double rad = atan2(fy_center, fx_center);// 计算预测的航向角
- if (abs(angles::shortest_angular_distance(rad0, rad)) < abs(angles::shortest_angular_distance(rad1, rad)))
+ if (armors_num != ArmorsNum::OUTPOST_3 && armors_num != ArmorsNum::BASE_1
+ && sqrt(pow(t_info.vx, 2) + pow(t_info.vy, 2) + pow(t_info.vz, 2)) > 0.3)
+ {
+ for (int i = 0; i < 2; i++)
+ {
+ PredictPosition();
+ CompensatePitch();
+ }
+ }
+ for (int i = 0; i < 4; i++)
{
- fx = armor[0].x;// 设置预测的x坐标为第一个护甲点的x坐标
- fy = armor[0].y;// 设置预测的y坐标为第一个护甲点的y坐标
- fz = armor[0].z;// 设置预测的z坐标为第一个护甲点的z坐标
- } else
+ PredictPose(armors_num);
+ CompensatePitch();
+ }
+ yout = (float) (g_yaw
+ + angles::shortest_angular_distance(g_yaw * CV_PI / 180, -atan2(fx_center, fy_center)) * 180 / CV_PI);
+ auto armor_yaw =
+ (float) (g_yaw + angles::shortest_angular_distance(g_yaw * CV_PI / 180, -atan2(fx, fy)) * 180 / CV_PI);
+ if (abs(armor_yaw - yout) > 2)
{
- fx = armor[1].x;// 设置预测的x坐标为第二个护甲点的x坐标
- fy = armor[1].y;// 设置预测的y坐标为第二个护甲点的y坐标
- fz = armor[1].z;// 设置预测的z坐标为第二个护甲点的z坐标
+ yaw_difference = DBL_MAX;
}
+ auto difference = yaw_difference;
+ send_data.pitch = pout;
+ send_data.yaw = yout;
+ return difference;
}
- else {
- fx = t_info.x - t_info.radius_1 * cos(t_info.yaw);
- fy = t_info.y - t_info.radius_1 * sin(t_info.yaw);
- fz = t_info.z;
- fx_center = fx;
- fy_center = fy;
+ void Compensator::PredictPosition()
+ {
+ fx = t_info.x + tmp_fly_time * t_info.vx - t_info.radius_1 * cos(t_info.yaw);
+ fy = t_info.y + tmp_fly_time * t_info.vy - t_info.radius_1 * sin(t_info.yaw);
+ fz = t_info.z + tmp_fly_time * t_info.vz;
+ }
+ void Compensator::PredictPose(ArmorsNum &armors_num)
+ {
+ if (abs(t_info.w_yaw) < 0.5)
+ {
+ if (armors_num == ArmorsNum::OUTPOST_3 || armors_num == ArmorsNum::BASE_1)
+ {
+ fx = calculateAverage(px) - t_info.radius_1 * cos(t_info.yaw);
+ fy = calculateAverage(py) - t_info.radius_1 * sin(t_info.yaw);
+ fz = calculateAverage(pz);
+ }
+ else
+ {
+ fx = t_info.x + tmp_fly_time * t_info.vx - t_info.radius_1 * cos(t_info.yaw);
+ fy = t_info.y + tmp_fly_time * t_info.vy - t_info.radius_1 * sin(t_info.yaw);
+ fz = t_info.z + tmp_fly_time * t_info.vz;
+ }
+ fx_center = fx;
+ fy_center = fy;
+ yaw_difference = 0;
+ }
+ else
+ {
+ float yaw;
+ if (armors_num == ArmorsNum::OUTPOST_3 || armors_num == ArmorsNum::BASE_1)
+ {
+ fx = calculateAverage(px);
+ fy = calculateAverage(py);
+ fz = calculateAverage(pz);
+ fv = calculateAverage(pv);
+ yaw = t_info.yaw + tmp_fly_time * fv;
+ }
+ else
+ {
+ fx = t_info.x + tmp_fly_time * t_info.vx;
+ fy = t_info.y + tmp_fly_time * t_info.vy;
+ fz = t_info.z + tmp_fly_time * t_info.vz;
+ yaw = t_info.yaw + tmp_fly_time * t_info.w_yaw;
+ }
+ fx_center = fx;
+ fy_center = fy;
+ float yaw_center = -atan2(fx_center, fy_center) + CV_PI / 2;
+ predict_armor armor1, armor2, armor3, armor4;
+ std::vector armors = {};
+ if (armors_num == ArmorsNum::OUTPOST_3)
+ {
+ armor1.position.x = fx - t_info.radius_1 * cos(yaw);
+ armor1.position.y = fy - t_info.radius_1 * sin(yaw);
+ armor1.position.z = fz;
+ armor1.yaw = abs(angles::shortest_angular_distance(yaw, yaw_center));
+ armor2.position.x = fx - t_info.radius_1 * cos(float(yaw + 2 * CV_PI / 3));
+ armor2.position.y = fy - t_info.radius_1 * sin(float(yaw + 2 * CV_PI / 3));
+ armor2.position.z = fz;
+ armor2.yaw = abs(angles::shortest_angular_distance(yaw + 2 * CV_PI / 3, yaw_center));
+ armor3.position.x = fx - t_info.radius_1 * sin(float(yaw + 4 * CV_PI / 3));
+ armor3.position.y = fy - t_info.radius_1 * cos(float(yaw + 4 * CV_PI / 3));
+ armor3.position.z = fz;
+ armor3.yaw = abs(angles::shortest_angular_distance(yaw + 4 * CV_PI / 3, yaw_center));
+ armors.push_back(armor1);
+ armors.push_back(armor2);
+ armors.push_back(armor3);
+ }
+ else if (armors_num == ArmorsNum::NORMAL_4)
+ {
+ armor1.position.x = fx - t_info.radius_1 * cos(yaw);
+ armor1.position.y = fy - t_info.radius_1 * sin(yaw);
+ armor1.position.z = fz;
+ armor1.yaw = abs(angles::shortest_angular_distance(yaw, yaw_center));
+ armor2.position.x = fx + t_info.radius_1 * cos(yaw);
+ armor2.position.y = fy + t_info.radius_1 * sin(yaw);
+ armor2.position.z = fz;
+ armor2.yaw = abs(angles::shortest_angular_distance(yaw + CV_PI, yaw_center));
+ armor3.position.x = fx - t_info.radius_2 * sin(yaw);
+ armor3.position.y = fy + t_info.radius_2 * cos(yaw);
+ armor3.position.z = fz + t_info.dz;
+ armor3.yaw = abs(angles::shortest_angular_distance(yaw + 3 * CV_PI / 2, yaw_center));
+ armor4.position.x = fx + t_info.radius_2 * sin(yaw);
+ armor4.position.y = fy - t_info.radius_2 * cos(yaw);
+ armor4.position.z = fz + t_info.dz;
+ armor4.yaw = abs(angles::shortest_angular_distance(yaw + CV_PI / 2, yaw_center));
+ armors.push_back(armor1);
+ armors.push_back(armor2);
+ armors.push_back(armor3);
+ armors.push_back(armor4);
+ }
+ else if (armors_num == ArmorsNum::BALANCE_2)
+ {
+ armor1.position.x = fx - t_info.radius_1 * cos(yaw);
+ armor1.position.y = fy - t_info.radius_1 * sin(yaw);
+ armor1.position.z = fz;
+ armor1.yaw = abs(angles::shortest_angular_distance(yaw, yaw_center));
+ armor2.position.x = fx + t_info.radius_1 * cos(yaw);
+ armor2.position.y = fy + t_info.radius_1 * sin(yaw);
+ armor2.position.z = fz;
+ armor2.yaw = abs(angles::shortest_angular_distance(yaw + CV_PI, yaw_center));
+ armors.push_back(armor1);
+ armors.push_back(armor2);
+ }
+ else if (armors_num == ArmorsNum::BASE_1)
+ {
+ armor1.position.x = fx - t_info.radius_1 * cos(yaw);
+ armor1.position.y = fy - t_info.radius_1 * sin(yaw);
+ armor1.position.z = fz;
+ armors.push_back(armor1);
+ }
+ sort(armors.begin(), armors.end(), cmp);
+ fx = armors[0].position.x;
+ fy = armors[0].position.y;
+ fz = armors[0].position.z;
+ yaw_difference = armors[0].yaw;
+ if (armors_num != ArmorsNum::BASE_1 && armors_num != ArmorsNum::OUTPOST_3 && abs(t_info.w_yaw) < 6)
+ {
+ fx_center = fx;
+ fy_center = fy;
+ yaw_difference = 0;
+ }
+ }
}
-}
-// 根据目标位置计算补偿角度和新的飞行时间
void Compensator::CompensatePitch()
{
- double dist_horizon = sqrt(pow(fx, 2) + pow(fy - imutofrt_y, 2));// 和目标在水平方向上的距离
- double target_height = fz-imutofrt_z;
+ double dist_horizon = sqrt(pow(fx, 2) + pow(fy - imutofrt_y, 2));
+ double target_height = fz - imutofrt_z;
// 迭代参数
double vx, vy, fly_time, tmp_height = target_height, delta_height = target_height, tmp_pitch, real_height;
int iteration_num = 0;
- while ((iteration_num <= 50 && abs(delta_height) >= 0.005) || iteration_num <= 5) {
+ while ((iteration_num <= 30 && abs(delta_height) >= 0.005) || iteration_num <= 5)
+ {
tmp_pitch = atan((tmp_height) / dist_horizon);
- dist_horizon = sqrt(pow(fx, 2) + pow(fy - imutofrt_y * cos(tmp_pitch)+imutofrt_z* sin(tmp_pitch), 2));
- target_height = fz -imutofrt_z * cos(tmp_pitch)-imutofrt_y* sin(tmp_pitch);
+ dist_horizon = sqrt(pow(fx, 2) + pow(fy - imutofrt_y * cos(tmp_pitch) + imutofrt_z * sin(tmp_pitch), 2));
+ target_height = fz - imutofrt_z * cos(tmp_pitch) - imutofrt_y * sin(tmp_pitch);
vx = bullet_speed_ * cos(tmp_pitch);
vy = bullet_speed_ * sin(tmp_pitch);
diff --git a/hnurm_armor/src/Processor.cpp b/hnurm_armor/src/Processor.cpp
index 0a289181a7fac937d51427db4012027409a1e66e..cb3556a8aefa294de56f31a9409db26f1c45689c 100755
--- a/hnurm_armor/src/Processor.cpp
+++ b/hnurm_armor/src/Processor.cpp
@@ -6,163 +6,189 @@
namespace hnurm
{
-void Processor::ProcessArmor(
- std::vector &armors_msg, hnurm_uart::msg::VisionRecvData &recv_data, TargetInfo &target_msg
-)
-{
- // Transform armor position from image frame to world coordinate
- auto now = recv_data.header.stamp;
- auto imu_to_gimbal = tf_buffer_->lookupTransform("gimbal_link", "imu_link", now, tf2::durationFromSec(0.1));
- for(auto &armor : armors_msg)
+ void Processor::ProcessArmor(
+ std::vector &armors_msg, hnurm_interfaces::msg::VisionRecvData &recv_data, TargetInfo &target_msg
+ )
{
- tsolver->GetTransformation(
- armor,
- (float)angles::from_degrees(recv_data.pitch),
- (float)angles::from_degrees(recv_data.yaw),
- imu_to_gimbal
- );
+ // Transform armor position from image frame to world coordinate
+ for (auto &armor: armors_msg)
+ {
+ tsolver->GetTransformation(
+ armor, (float) angles::from_degrees(recv_data.pitch), (float) angles::from_degrees(recv_data.yaw)
+ );
+ }
+
+ auto time = std::chrono::steady_clock::now();
+
+ if (tracker->tracker_state == Tracker::LOST)
+ {
+ tracker->init(armors_msg);
+ }
+ else
+ {
+ dt_ = (double) std::chrono::duration_cast(time - time_elapsed_).count() / 1e6;
+ tracker->update(armors_msg);
+ }
+ time_elapsed_ = time;
+ const auto state = tracker->target_state;
+ // clang-format off
+ target_msg.x = (float) state(0);
+ target_msg.vx = (float) state(1);
+ target_msg.y = (float) state(2);
+ target_msg.vy = (float) state(3);
+ target_msg.z = (float) state(4);
+ target_msg.vz = (float) state(5);
+ target_msg.yaw = (float) state(6);
+ target_msg.w_yaw = (float) state(7);
+ target_msg.radius_1 = (float) state(8);
+ target_msg.radius_2 = (float) tracker->another_r;
+ target_msg.dz = (float) tracker->dz;
+ // clang-format on
+ if (!armors_msg.empty())
+ {
+ if (target_msg.w_yaw < 1)
+ {
+ auto gimbal_yaw = recv_data.yaw;
+ std::sort(armors_msg.begin(), armors_msg.end(), [&](const Armor &armor1, const Armor &armor2)
+ {
+ Eigen::Vector3d euler1, euler2;
+ this->tsolver->rotationMatrixToEulerAngles(armor1.rotation, euler1);
+ this->tsolver->rotationMatrixToEulerAngles(armor2.rotation, euler2);
+ return abs(euler1(1) - gimbal_yaw) < abs(euler2(1) - gimbal_yaw);
+ });
+
+ target_msg.x = armors_msg[0].position.x();
+ target_msg.y = armors_msg[0].position.y();
+ target_msg.z = armors_msg[0].position.z();
+ }
+ if (Eigen::Vector3f(target_msg.vx, target_msg.vy, target_msg.vz).norm() < 0.5)
+ {
+ target_msg.vx = 0;
+ target_msg.vy = 0;
+ target_msg.vz = 0;
+ }
+ }
}
- auto time = std::chrono::steady_clock::now();
-
- if(tracker->tracker_state == Tracker::LOST)
+ Processor::Processor(rclcpp::Node::SharedPtr node)
+ : node_(node)
{
- tracker->init(armors_msg);
- }
- else
- {
- dt_ = (double)std::chrono::duration_cast(time - time_elapsed_).count() / 1e6;
- tracker->update(armors_msg);
- }
- time_elapsed_ = time;
- const auto state = tracker->target_state;
- // clang-format off
- target_msg.x = (float )state(0);
- target_msg.vx = (float )state(1);
- target_msg.y = (float )state(2);
- target_msg.vy = (float )state(3);
- target_msg.z = (float )state(4);
- target_msg.vz = (float )state(5);
- target_msg.yaw = (float )state(6);
- target_msg.w_yaw = (float )state(7);
- target_msg.radius_1 = (float )state(8);
- target_msg.radius_2 = (float )tracker->another_r;
- target_msg.dz = (float )tracker->dz;
- // clang-format on
-}
-
-Processor::Processor(rclcpp::Node::SharedPtr node) : node_(node)
-{
- dt_ = 0.01;
-
- tracker = std::make_unique(node_);
- tsolver = std::make_unique(node_);
-
- // EKF
- // xa = x_armor, xc = x_robot_center
- // state: xc, v_xc, yc, v_yc, za, v_za, yaw, v_yaw, r
- // measurement: xa, ya, za, yaw
- // f - Process function
- auto f = [this](const Eigen::VectorXd &x) {
- Eigen::VectorXd x_new = x;
- x_new(0) += x(1) * dt_;
- x_new(2) += x(3) * dt_;
- x_new(4) += x(5) * dt_;
- x_new(6) += x(7) * dt_;
- return x_new;
- };
-
- // J_f - Jacobian of process function
- auto j_f = [this](const Eigen::VectorXd &) {
- Eigen::MatrixXd f(9, 9);
- // clang-format off
- f << 1, dt_, 0, 0, 0, 0, 0, 0, 0,
- 0, 1, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 1, dt_, 0, 0, 0, 0, 0,
- 0, 0, 0, 1, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 1, dt_, 0, 0, 0,
- 0, 0, 0, 0, 0, 1, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 1, dt_, 0,
- 0, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 1;
- // clang-format on
- return f;
- };
-
- // h - Observation function
- auto h = [](const Eigen::VectorXd &x) {
- Eigen::VectorXd z(4);
- double xc = x(0), yc = x(2), yaw = x(6), r = x(8);
- z(0) = xc - r * cos(yaw); // xa
- z(1) = yc - r * sin(yaw); // ya
- z(2) = x(4); // za
- z(3) = x(6); // yaw
- return z;
- };
- // J_h - Jacobian of observation function
- auto j_h = [](const Eigen::VectorXd &x) {
- Eigen::MatrixXd h(4, 9);
- double yaw = x(6), r = x(8);
- // clang-format off
+ dt_ = 0.01;
+
+ tracker = std::make_unique(node_);
+ tsolver = std::make_unique(node_);
+
+ // EKF
+ // xa = x_armor, xc = x_robot_center
+ // state: xc, v_xc, yc, v_yc, za, v_za, yaw, v_yaw, r
+ // measurement: xa, ya, za, yaw
+ // f - Process function
+ auto f = [this](const Eigen::VectorXd &x)
+ {
+ Eigen::VectorXd x_new = x;
+ x_new(0) += x(1) * dt_;
+ x_new(2) += x(3) * dt_;
+ x_new(4) += x(5) * dt_;
+ x_new(6) += x(7) * dt_;
+ return x_new;
+ };
+
+ // J_f - Jacobian of process function
+ auto j_f = [this](const Eigen::VectorXd &)
+ {
+ Eigen::MatrixXd f(9, 9);
+ // clang-format off
+ f << 1, dt_, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 1, dt_, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, dt_, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 1, dt_, 0,
+ 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1;
+ // clang-format on
+ return f;
+ };
+
+ // h - Observation function
+ auto h = [](const Eigen::VectorXd &x)
+ {
+ Eigen::VectorXd z(4);
+ double xc = x(0), yc = x(2), yaw = x(6), r = x(8);
+ z(0) = xc - r * cos(yaw); // xa
+ z(1) = yc - r * sin(yaw); // ya
+ z(2) = x(4); // za
+ z(3) = x(6); // yaw
+ return z;
+ };
+ // J_h - Jacobian of observation function
+ auto j_h = [](const Eigen::VectorXd &x)
+ {
+ Eigen::MatrixXd h(4, 9);
+ double yaw = x(6), r = x(8);
+ // clang-format off
// xc v_xc yc v_yc za v_za yaw v_yaw r
- h << 1, 0, 0, 0, 0, 0, r*sin(yaw), 0, -cos(yaw),
- 0, 0, 1, 0, 0, 0, -r*cos(yaw),0, -sin(yaw),
- 0, 0, 0, 0, 1, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 1, 0, 0;
- // clang-format on
- return h;
- };
-
- // tracker_config_node["s2_q_xyz"] >> s2qxyz_;
- // tracker_config_node["s2_q_yaw"] >> s2qyaw_;
- // tracker_config_node["s2_q_r"] >> s2qr_;
- s2qxyz_ = node->declare_parameter("processor.tracker.s2_q_xyz", 0.05);
- s2qyaw_ = node->declare_parameter("processor.tracker.s2_q_yaw", 5.0);
- s2qr_ = node->declare_parameter("processor.tracker.s2_q_r", 80.0);
-
- // update_Q - process noise covariance matrix
- auto u_q = [this]() {
- Eigen::MatrixXd q(9, 9);
- double t = dt_, x = s2qxyz_, y = s2qyaw_, r = s2qr_;
- double q_x_x = pow(t, 4) / 4 * x, q_x_vx = pow(t, 3) / 2 * x, q_vx_vx = pow(t, 2) * x;
- double q_y_y = pow(t, 4) / 4 * y, q_y_vy = pow(t, 3) / 2 * x, q_vy_vy = pow(t, 2) * y;
- double q_r = pow(t, 4) / 4 * r;
- // clang-format off
+ h << 1, 0, 0, 0, 0, 0, r * sin(yaw), 0, -cos(yaw),
+ 0, 0, 1, 0, 0, 0, -r * cos(yaw), 0, -sin(yaw),
+ 0, 0, 0, 0, 1, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 1, 0, 0;
+ // clang-format on
+ return h;
+ };
+
+ // tracker_config_node["s2_q_xyz"] >> s2qxyz_;
+ // tracker_config_node["s2_q_yaw"] >> s2qyaw_;
+ // tracker_config_node["s2_q_r"] >> s2qr_;
+ s2qxyz_ = node->declare_parameter("processor.tracker.s2_q_xyz", 0.05);
+ s2qyaw_ = node->declare_parameter("processor.tracker.s2_q_yaw", 5.0);
+ s2qr_ = node->declare_parameter("processor.tracker.s2_q_r", 80.0);
+
+ // update_Q - process noise covariance matrix
+ auto u_q = [this]()
+ {
+ Eigen::MatrixXd q(9, 9);
+ double t = dt_, x = s2qxyz_, y = s2qyaw_, r = s2qr_;
+ double q_x_x = pow(t, 4) / 4 * x, q_x_vx = pow(t, 3) / 2 * x, q_vx_vx = pow(t, 2) * x;
+ double q_y_y = pow(t, 4) / 4 * y, q_y_vy = pow(t, 3) / 2 * x, q_vy_vy = pow(t, 2) * y;
+ double q_r = pow(t, 4) / 4 * r;
+ // clang-format off
// xc v_xc yc v_yc za v_za yaw v_yaw r
- q << q_x_x, q_x_vx, 0, 0, 0, 0, 0, 0, 0,
- q_x_vx, q_vx_vx,0, 0, 0, 0, 0, 0, 0,
- 0, 0, q_x_x, q_x_vx, 0, 0, 0, 0, 0,
- 0, 0, q_x_vx, q_vx_vx,0, 0, 0, 0, 0,
- 0, 0, 0, 0, q_x_x, q_x_vx, 0, 0, 0,
- 0, 0, 0, 0, q_x_vx, q_vx_vx,0, 0, 0,
- 0, 0, 0, 0, 0, 0, q_y_y, q_y_vy, 0,
- 0, 0, 0, 0, 0, 0, q_y_vy, q_vy_vy,0,
- 0, 0, 0, 0, 0, 0, 0, 0, q_r;
- // clang-format on
- return q;
- };
-
- r_xyz_factor = node->declare_parameter("processor.tracker.r_xyz_factor", 4e-4);
- r_yaw = node->declare_parameter("processor.tracker.r_yaw", 5e-3);
-
- // update_R - measurement noise covariance matrix
- auto u_r = [this](const Eigen::VectorXd &z) {
- Eigen::DiagonalMatrix r;
- double x = r_xyz_factor;
- r.diagonal() << abs(x * z[0]), abs(x * z[1]), abs(x * z[2]), r_yaw;
- return r;
- };
-
- // P - error estimate covariance matrix
- Eigen::DiagonalMatrix p0;
- p0.setIdentity();
-
- // init ekf
- tracker->ekf = ExtendedKalmanFilter {f, h, j_f, j_h, u_q, u_r, p0};
-
- tf_buffer_ = std::make_shared(node_->get_clock());
- tf_listener_ = std::make_shared(*tf_buffer_);
-}
+ q << q_x_x, q_x_vx, 0, 0, 0, 0, 0, 0, 0,
+ q_x_vx, q_vx_vx, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, q_x_x, q_x_vx, 0, 0, 0, 0, 0,
+ 0, 0, q_x_vx, q_vx_vx, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, q_x_x, q_x_vx, 0, 0, 0,
+ 0, 0, 0, 0, q_x_vx, q_vx_vx, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, q_y_y, q_y_vy, 0,
+ 0, 0, 0, 0, 0, 0, q_y_vy, q_vy_vy, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, q_r;
+ // clang-format on
+ return q;
+ };
+
+ r_xyz_factor = node->declare_parameter("processor.tracker.r_xyz_factor", 4e-4);
+ r_yaw = node->declare_parameter("processor.tracker.r_yaw", 5e-3);
+
+ // update_R - measurement noise covariance matrix
+ auto u_r = [this](const Eigen::VectorXd &z)
+ {
+ Eigen::DiagonalMatrix r;
+ double x = r_xyz_factor;
+ r.diagonal() << abs(x * z[0]), abs(x * z[1]), abs(x * z[2]), r_yaw;
+ return r;
+ };
+
+ // P - error estimate covariance matrix
+ Eigen::DiagonalMatrix p0;
+ p0.setIdentity();
+
+ // init ekf
+ tracker->ekf = ExtendedKalmanFilter{f, h, j_f, j_h, u_q, u_r, p0};
+
+ tf_buffer_ = std::make_shared(node_->get_clock());
+ tf_listener_ = std::make_shared(*tf_buffer_);
+ }
} // namespace hnurm
\ No newline at end of file
diff --git a/hnurm_armor/src/armor_node.cpp b/hnurm_armor/src/armor_node.cpp
index f2c742be8902438fe947b2060ab1df28faa543af..39ec4f6b82de21777dd9a934492cbb4a342c401b 100644
--- a/hnurm_armor/src/armor_node.cpp
+++ b/hnurm_armor/src/armor_node.cpp
@@ -5,304 +5,337 @@
#include
#include
+using namespace std::chrono_literals;
+
namespace hnurm
{
-ArmorNode::ArmorNode(const rclcpp::NodeOptions &options) : Node("armor_node", options)
-{
- std::string tracker_info_topic = this->declare_parameter("tracker_info_topic", "tracker_info");
- std::string target_topic = this->declare_parameter("target_topic", "target");
- std::string vision_send_topic = this->declare_parameter("vision_send_topic", "vision_send_data");
+ ArmorNode::ArmorNode(const rclcpp::NodeOptions &options)
+ : Node("armor_node", options)
+ {
+ std::string tracker_info_topic = this->declare_parameter("tracker_info_topic", "tracker_info");
+ std::string target_topic = this->declare_parameter("target_topic", "target");
+ std::string vision_send_topic = this->declare_parameter("vision_send_topic", "vision_send_data");
- RCLCPP_INFO(get_logger(), "ArmorNode is running");
- processor_ = std::make_unique(shared_from_this());
- RCLCPP_INFO(get_logger(), "Processor created");
- compensator_ = std::make_unique(shared_from_this());
- RCLCPP_INFO(get_logger(), "Compensator created");
+ RCLCPP_INFO(get_logger(), "ArmorNode is running");
+ processor_ = std::make_unique(shared_from_this());
+ RCLCPP_INFO(get_logger(), "Processor created");
+ compensator_ = std::make_unique(shared_from_this());
+ RCLCPP_INFO(get_logger(), "Compensator created");
- send_data_pub_ = this->create_publisher(vision_send_topic, rclcpp::ServicesQoS());
+ send_data_pub_
+ = this->create_publisher(vision_send_topic, rclcpp::ServicesQoS());
- tracker_info_pub_
- = this->create_publisher(tracker_info_topic, rclcpp::SensorDataQoS());
- target_pub_ = this->create_publisher(target_topic, rclcpp::SensorDataQoS());
+ tracker_info_pub_
+ = this->create_publisher(tracker_info_topic, rclcpp::SensorDataQoS());
+ target_pub_ = this->create_publisher(target_topic, rclcpp::SensorDataQoS());
- armor_array_sub_ = this->create_subscription(
- "armor", rclcpp::SensorDataQoS(), std::bind(&ArmorNode::armor_array_callback, this, std::placeholders::_1)
- );
+ armor_array_sub_ = this->create_subscription(
+ "armor", rclcpp::SensorDataQoS(), std::bind(&ArmorNode::armor_array_callback, this, std::placeholders::_1)
+ );
- init_markers();
-}
+ auto using_timer = this->declare_parameter("using_timer", false);
+ if (using_timer)
+ timer_ = this->create_wall_timer(1ms, std::bind(&ArmorNode::timer_callback, this));
-void ArmorNode::init_markers()
-{
- position_marker_.ns = "position";
- position_marker_.type = visualization_msgs::msg::Marker::SPHERE;
- position_marker_.scale.x = position_marker_.scale.y = position_marker_.scale.z = 0.1;
- position_marker_.color.a = 1.0;
- position_marker_.color.g = 1.0;
- linear_v_marker_.type = visualization_msgs::msg::Marker::ARROW;
- linear_v_marker_.ns = "linear_v";
- linear_v_marker_.scale.x = 0.03;
- linear_v_marker_.scale.y = 0.05;
- linear_v_marker_.color.a = 1.0;
- linear_v_marker_.color.r = 1.0;
- linear_v_marker_.color.g = 1.0;
- angular_v_marker_.type = visualization_msgs::msg::Marker::ARROW;
- angular_v_marker_.ns = "angular_v";
- angular_v_marker_.scale.x = 0.03;
- angular_v_marker_.scale.y = 0.05;
- angular_v_marker_.color.a = 1.0;
- angular_v_marker_.color.b = 1.0;
- angular_v_marker_.color.g = 1.0;
- armor_marker_.ns = "armors";
- armor_marker_.type = visualization_msgs::msg::Marker::CUBE;
- armor_marker_.scale.x = 0.03;
- armor_marker_.scale.z = 0.125;
- armor_marker_.color.a = 1.0;
- armor_marker_.color.r = 1.0;
- std::string topic = this->declare_parameter("marker_topic", "/tracker/marker");
- marker_pub_ = this->create_publisher(topic, 10);
-}
-
-void ArmorNode::armor_array_callback(const hnurm_detect::msg::ArmorArray::SharedPtr msg)
-{
- TargetInfo tmp_target;
-
- auto armors = fromROSMsg(*msg);
-
- // update processor and tracker
- processor_->ProcessArmor(armors, msg->recv_uart, tmp_target);
-
- // send target info over uart
- send_target(tmp_target, msg->recv_uart);
-
- // publish info and markers
- // tracker info
- hnurm_armor::msg::TrackerInfo info_msg;
- hnurm_armor::msg::Target target_msg;
- target_msg.header.stamp = msg->header.stamp;
- target_msg.header.frame_id = "odom";
-
- info_msg.position_diff = processor_->tracker->info_position_diff;
- info_msg.yaw_diff = processor_->tracker->info_yaw_diff;
- info_msg.position.x = processor_->tracker->measurement(0);
- info_msg.position.y = processor_->tracker->measurement(1);
- info_msg.position.z = processor_->tracker->measurement(2);
- info_msg.yaw = processor_->tracker->measurement(3);
- info_msg.yaw = angles::to_degrees(info_msg.yaw);
- info_msg.distance = processor_->tracker->measurement.norm();
- tracker_info_pub_->publish(info_msg);
-
- if(processor_->tracker->tracker_state == Tracker::DETECTING)
+ init_markers();
+ }
+ float ArmorNode::calculateAverage(const std::vector &vec)
{
- target_msg.tracking = false;
+ int n = vec.size();
+ if (n == 0) return 0.0f;
+
+ float sum = 0.0f;
+ for (float value: vec)
+ {
+ sum += value;
+ }
+ return sum / n;
}
- else if(processor_->tracker->tracker_state == Tracker::TRACKING || processor_->tracker->tracker_state == Tracker::TEMP_LOST)
+ void ArmorNode::Averagedistance(std::vector &p, float value)
{
- target_msg.tracking = true;
- // Fill target message
- const auto &state = processor_->tracker->target_state;
- target_msg.id = processor_->tracker->tracked_id;
- target_msg.armors_num = static_cast(processor_->tracker->tracked_armors_num);
- target_msg.position.x = state(0);
- target_msg.velocity.x = state(1);
- target_msg.position.y = state(2);
- target_msg.velocity.y = state(3);
- target_msg.position.z = state(4);
- target_msg.velocity.z = state(5);
- target_msg.yaw = state(6);
- target_msg.v_yaw = state(7);
- target_msg.radius_1 = state(8);
- target_msg.radius_2 = processor_->tracker->another_r;
- target_msg.dz = processor_->tracker->dz;
+ if (p.size() >= 50)
+ {
+ p.erase(p.begin());
+ }
+ p.push_back(value);
}
- target_pub_->publish(target_msg);
+ void ArmorNode::timer_callback()
+ {
+ std::lock_guard lock(mtx);
+ TargetInfo tmp_target;
- publish_markers(target_msg);
-}
+ // update processor and tracker
+ processor_->ProcessArmor(armors, last_recv_array_.recv_uart, tmp_target);
-void ArmorNode::send_target(TargetInfo &target, hnurm_uart::msg::VisionRecvData &uart_msg)
-{
- hnurm_uart::msg::VisionSendData send;
- send.header.frame_id = "serial_send";
- send.header.stamp = this->now();
- send.control_id = uart_msg.control_id; // copy the control id
+ // send target info over uart
+ send_target(tmp_target, last_recv_array_.recv_uart);
+ }
- if(processor_->tracker->tracker_state != Tracker::LOST)
+ void ArmorNode::init_markers()
{
- float armor_yaw=compensator_->CalcFinalAngle(target, uart_msg, send, processor_->tracker->tracked_armors_num);
- //转的飞快的目标,瞄中间,不跟装甲
- if (abs(target.w_yaw)> 7) {
- if (abs(send.yaw - uart_msg.yaw) < 2 && abs(send.pitch - uart_msg.pitch) < 0.5 && abs(send.yaw - armor_yaw) < 2 )
- {
- send.target_state.data = hnurm_uart::msg::TargetState::FIRE;
- } else
- {
- send.target_state.data = hnurm_uart::msg::TargetState::CONVERGING;
- }
- }
- //转的不快的目标,跟装甲
- else {
- send.yaw = armor_yaw;
- if (abs(send.yaw - uart_msg.yaw) < 2 && abs(send.pitch - uart_msg.pitch) < 0.8)
- {
- send.target_state.data = hnurm_uart::msg::TargetState::FIRE;
- } else
- {
- send.target_state.data = hnurm_uart::msg::TargetState::CONVERGING;
- }
- }
- send.target_distance = Eigen::Vector3f(target.x, target.y, target.z).norm();
- //添加偏置环节
- send.pitch += compensator_->pitch_bias_;
- send.pitch += compensator_->pitch_bias_;
- send_data_pub_->publish(send);
+ position_marker_.ns = "position";
+ position_marker_.type = visualization_msgs::msg::Marker::SPHERE;
+ position_marker_.scale.x = position_marker_.scale.y = position_marker_.scale.z = 0.1;
+ position_marker_.color.a = 1.0;
+ position_marker_.color.g = 1.0;
+ linear_v_marker_.type = visualization_msgs::msg::Marker::ARROW;
+ linear_v_marker_.ns = "linear_v";
+ linear_v_marker_.scale.x = 0.03;
+ linear_v_marker_.scale.y = 0.05;
+ linear_v_marker_.color.a = 1.0;
+ linear_v_marker_.color.r = 1.0;
+ linear_v_marker_.color.g = 1.0;
+ angular_v_marker_.type = visualization_msgs::msg::Marker::ARROW;
+ angular_v_marker_.ns = "angular_v";
+ angular_v_marker_.scale.x = 0.03;
+ angular_v_marker_.scale.y = 0.05;
+ angular_v_marker_.color.a = 1.0;
+ angular_v_marker_.color.b = 1.0;
+ angular_v_marker_.color.g = 1.0;
+ armor_marker_.ns = "armors";
+ armor_marker_.type = visualization_msgs::msg::Marker::CUBE;
+ armor_marker_.scale.x = 0.03;
+ armor_marker_.scale.z = 0.125;
+ armor_marker_.color.a = 1.0;
+ armor_marker_.color.r = 1.0;
+ std::string topic = this->declare_parameter("marker_topic", "/tracker/marker");
+ marker_pub_ = this->create_publisher(topic, 10);
}
- else
+
+ void ArmorNode::armor_array_callback(const hnurm_interfaces::msg::ArmorArray::SharedPtr msg)
{
- send.target_state.data = hnurm_uart::msg::TargetState::LOST_TARGET;
- send_data_pub_->publish(send);
- }
-}
+ std::lock_guard lock(mtx);
+ last_recv_array_ = *msg;
-void ArmorNode::publish_markers(hnurm_armor::msg::Target &target_msg)
-{
- position_marker_.header = target_msg.header;
- linear_v_marker_.header = target_msg.header;
- angular_v_marker_.header = target_msg.header;
- armor_marker_.header = target_msg.header;
+ TargetInfo tmp_target;
+
+ armors = fromROSMsg(*msg);
+
+ // update processor and tracker
+ processor_->ProcessArmor(armors, msg->recv_uart, tmp_target);
+
+ // send target info over uart
+ send_target(tmp_target, msg->recv_uart);
+
+ // publish info and markers
+ // tracker info
+ hnurm_interfaces::msg::TrackerInfo info_msg;
+ hnurm_interfaces::msg::Target target_msg;
+ target_msg.header.stamp = msg->header.stamp;
+ target_msg.header.frame_id = "odom";
- visualization_msgs::msg::MarkerArray marker_array;
- if(target_msg.tracking)
+ info_msg.position_diff = processor_->tracker->info_position_diff;
+ info_msg.yaw_diff = processor_->tracker->info_yaw_diff;
+ info_msg.position.x = processor_->tracker->measurement(0);
+ info_msg.position.y = processor_->tracker->measurement(1);
+ info_msg.position.z = processor_->tracker->measurement(2);
+ info_msg.yaw = processor_->tracker->measurement(3);
+ info_msg.yaw = angles::to_degrees(info_msg.yaw);
+ info_msg.distance = processor_->tracker->measurement.norm();
+ tracker_info_pub_->publish(info_msg);
+
+ if (processor_->tracker->tracker_state == Tracker::DETECTING)
+ {
+ target_msg.tracking = false;
+ }
+ else if (processor_->tracker->tracker_state == Tracker::TRACKING
+ || processor_->tracker->tracker_state == Tracker::TEMP_LOST)
+ {
+ target_msg.tracking = true;
+ // Fill target message
+ const auto &state = processor_->tracker->target_state;
+ Averagedistance(pv, state(7));
+ target_msg.id = processor_->tracker->tracked_id;
+ target_msg.armors_num = static_cast(processor_->tracker->tracked_armors_num);
+ target_msg.position.x = state(0);
+ target_msg.velocity.x = state(1);
+ target_msg.position.y = state(2);
+ target_msg.velocity.y = state(3);
+ target_msg.position.z = state(4);
+ target_msg.velocity.z = state(5);
+ target_msg.yaw = state(6);
+ target_msg.v_yaw = calculateAverage(pv);
+ target_msg.radius_1 = state(8);
+ target_msg.radius_2 = processor_->tracker->another_r;
+ target_msg.dz = processor_->tracker->dz;
+ target_msg.armor_pitch = processor_->tracker->armor_pitch;
+ target_msg.armor_yaw = processor_->tracker->armor_yaw;
+ }
+ if (!armors.empty())
+ {
+ target_pub_->publish(target_msg);
+ }
+
+ publish_markers(target_msg);
+ }
+
+ void ArmorNode::send_target(TargetInfo &target, hnurm_interfaces::msg::VisionRecvData &uart_msg)
{
- double yaw = target_msg.yaw, r1 = target_msg.radius_1, r2 = target_msg.radius_2;
- double xc = target_msg.position.x, yc = target_msg.position.y, za = target_msg.position.z;
- double vx = target_msg.velocity.x, vy = target_msg.velocity.y, vz = target_msg.velocity.z;
- double dz = target_msg.dz;
-
- position_marker_.action = visualization_msgs::msg::Marker::ADD;
- position_marker_.pose.position.x = xc;
- position_marker_.pose.position.y = yc;
- position_marker_.pose.position.z = za + dz / 2;
-
- linear_v_marker_.action = visualization_msgs::msg::Marker::ADD;
- linear_v_marker_.points.clear();
- linear_v_marker_.points.emplace_back(position_marker_.pose.position);
- geometry_msgs::msg::Point arrow_end = position_marker_.pose.position;
- arrow_end.x += vx;
- arrow_end.y += vy;
- arrow_end.z += vz;
- linear_v_marker_.points.emplace_back(arrow_end);
-
- angular_v_marker_.action = visualization_msgs::msg::Marker::ADD;
- angular_v_marker_.points.clear();
- angular_v_marker_.points.emplace_back(position_marker_.pose.position);
- arrow_end = position_marker_.pose.position;
- arrow_end.z += target_msg.v_yaw / M_PI;
- angular_v_marker_.points.emplace_back(arrow_end);
-
- armor_marker_.action = visualization_msgs::msg::Marker::ADD;
- armor_marker_.scale.y = processor_->tracker->tracked_armor.armor_type == ArmorType::SMALL ? 0.135 : 0.23;
- bool is_current_pair = true;
- size_t a_n = target_msg.armors_num;
- geometry_msgs::msg::Point p_a;
- double r = 0;
- for(size_t i = 0; i < a_n; i++)
+ hnurm_interfaces::msg::VisionSendData send;
+ send.header.frame_id = "serial_send";
+ send.header.stamp = this->now();
+ send.control_id = uart_msg.control_id; // copy the control id
+ if (processor_->tracker->tracker_state != Tracker::LOST && processor_->tracker->tracker_state != Tracker::JUMP)
{
- double tmp_yaw = yaw + i * (2 * M_PI / a_n);
- // Only 4 armors has 2 radius and height
- if(a_n == 4)
+ float armor_yaw =
+ compensator_->CalcFinalAngle(target, uart_msg, send, processor_->tracker->tracked_armors_num);
+ if (abs(send.yaw - uart_msg.yaw) < 1 && abs(send.pitch - uart_msg.pitch) < 0.5 && armor_yaw < 30)
{
- r = is_current_pair ? r1 : r2;
- p_a.z = za + (is_current_pair ? 0 : dz);
- is_current_pair = !is_current_pair;
+ send.target_state.data = hnurm_interfaces::msg::TargetState::FIRE;
}
else
{
- r = r1;
- p_a.z = za;
+ send.target_state.data = hnurm_interfaces::msg::TargetState::CONVERGING;
}
- p_a.x = xc - r * cos(tmp_yaw);
- p_a.y = yc - r * sin(tmp_yaw);
-
- armor_marker_.id = i;
- armor_marker_.pose.position = p_a;
- tf2::Quaternion q;
- q.setRPY(0, target_msg.id == "outpost" ? -0.26 : 0.26, tmp_yaw);
- armor_marker_.pose.orientation = tf2::toMsg(q);
- marker_array.markers.emplace_back(armor_marker_);
+ send.target_distance = Eigen::Vector3f(target.x, target.y, target.z).norm();
+ send_data_pub_->publish(send);
+ }
+ else
+ {
+ send.target_state.data = hnurm_interfaces::msg::TargetState::LOST_TARGET;
+ send_data_pub_->publish(send);
}
}
- else
+
+ void ArmorNode::publish_markers(hnurm_interfaces::msg::Target &target_msg)
{
- position_marker_.action = visualization_msgs::msg::Marker::DELETE;
- linear_v_marker_.action = visualization_msgs::msg::Marker::DELETE;
- angular_v_marker_.action = visualization_msgs::msg::Marker::DELETE;
+ position_marker_.header = target_msg.header;
+ linear_v_marker_.header = target_msg.header;
+ angular_v_marker_.header = target_msg.header;
+ armor_marker_.header = target_msg.header;
- armor_marker_.action = visualization_msgs::msg::Marker::DELETE;
- marker_array.markers.emplace_back(armor_marker_);
- }
+ visualization_msgs::msg::MarkerArray marker_array;
+ if (target_msg.tracking)
+ {
+ double yaw = target_msg.yaw, r1 = target_msg.radius_1, r2 = target_msg.radius_2;
+ double xc = target_msg.position.x, yc = target_msg.position.y, za = target_msg.position.z;
+ double vx = target_msg.velocity.x, vy = target_msg.velocity.y, vz = target_msg.velocity.z;
+ double dz = target_msg.dz;
+
+ position_marker_.action = visualization_msgs::msg::Marker::ADD;
+ position_marker_.pose.position.x = xc;
+ position_marker_.pose.position.y = yc;
+ position_marker_.pose.position.z = za + dz / 2;
+
+ linear_v_marker_.action = visualization_msgs::msg::Marker::ADD;
+ linear_v_marker_.points.clear();
+ linear_v_marker_.points.emplace_back(position_marker_.pose.position);
+ geometry_msgs::msg::Point arrow_end = position_marker_.pose.position;
+ arrow_end.x += vx;
+ arrow_end.y += vy;
+ arrow_end.z += vz;
+ linear_v_marker_.points.emplace_back(arrow_end);
+
+ angular_v_marker_.action = visualization_msgs::msg::Marker::ADD;
+ angular_v_marker_.points.clear();
+ angular_v_marker_.points.emplace_back(position_marker_.pose.position);
+ arrow_end = position_marker_.pose.position;
+ arrow_end.z += target_msg.v_yaw / M_PI;
+ angular_v_marker_.points.emplace_back(arrow_end);
+
+ armor_marker_.action = visualization_msgs::msg::Marker::ADD;
+ armor_marker_.scale.y = processor_->tracker->tracked_armor.armor_type == ArmorType::SMALL ? 0.135 : 0.23;
+ bool is_current_pair = true;
+ size_t a_n = target_msg.armors_num;
+ geometry_msgs::msg::Point p_a;
+ double r = 0;
+ for (size_t i = 0; i < a_n; i++)
+ {
+ double tmp_yaw = yaw + i * (2 * M_PI / a_n);
+ // Only 4 armors has 2 radius and height
+ if (a_n == 4)
+ {
+ r = is_current_pair ? r1 : r2;
+ p_a.z = za + (is_current_pair ? 0 : dz);
+ is_current_pair = !is_current_pair;
+ }
+ else
+ {
+ r = r1;
+ p_a.z = za;
+ }
+ p_a.x = xc - r * cos(tmp_yaw);
+ p_a.y = yc - r * sin(tmp_yaw);
+
+ armor_marker_.id = i;
+ armor_marker_.pose.position = p_a;
+ tf2::Quaternion q;
+ q.setRPY(0, target_msg.id == "outpost" ? -0.26 : 0.26, tmp_yaw);
+ armor_marker_.pose.orientation = tf2::toMsg(q);
+ marker_array.markers.emplace_back(armor_marker_);
+ }
+ }
+ else
+ {
+ position_marker_.action = visualization_msgs::msg::Marker::DELETE;
+ linear_v_marker_.action = visualization_msgs::msg::Marker::DELETE;
+ angular_v_marker_.action = visualization_msgs::msg::Marker::DELETE;
+
+ armor_marker_.action = visualization_msgs::msg::Marker::DELETE;
+ marker_array.markers.emplace_back(armor_marker_);
+ }
- marker_array.markers.emplace_back(position_marker_);
- marker_array.markers.emplace_back(linear_v_marker_);
- marker_array.markers.emplace_back(angular_v_marker_);
- marker_pub_->publish(marker_array);
-}
+ marker_array.markers.emplace_back(position_marker_);
+ marker_array.markers.emplace_back(linear_v_marker_);
+ marker_array.markers.emplace_back(angular_v_marker_);
+ marker_pub_->publish(marker_array);
+ }
-std::vector fromROSMsg(const hnurm_detect::msg::ArmorArray &msg)
-{
- std::vector armors;
- armors.resize(msg.armors.size());
- for(size_t i = 0; i < msg.armors.size(); i++)
+ std::vector fromROSMsg(const hnurm_interfaces::msg::ArmorArray &msg)
{
- armors[i].left_light.color = msg.armors[i].left_light.color;
- armors[i].left_light.top.x = msg.armors[i].left_light.top.x;
- armors[i].left_light.top.y = msg.armors[i].left_light.top.y;
- armors[i].left_light.center.x = msg.armors[i].left_light.center.x;
- armors[i].left_light.center.y = msg.armors[i].left_light.center.y;
- armors[i].left_light.bottom.x = msg.armors[i].left_light.bottom.x;
- armors[i].left_light.bottom.y = msg.armors[i].left_light.bottom.y;
- armors[i].left_light.size.width = msg.armors[i].left_light.size.x;
- armors[i].left_light.size.height = msg.armors[i].left_light.size.y;
- armors[i].left_light.length = msg.armors[i].left_light.length;
- armors[i].left_light.width = msg.armors[i].left_light.width;
- armors[i].left_light.tilt_angle = msg.armors[i].left_light.tilt_angle;
- armors[i].left_light.angle = msg.armors[i].left_light.angle;
-
- armors[i].right_light.color = msg.armors[i].right_light.color;
- armors[i].right_light.top.x = msg.armors[i].right_light.top.x;
- armors[i].right_light.top.y = msg.armors[i].right_light.top.y;
- armors[i].right_light.center.x = msg.armors[i].right_light.center.x;
- armors[i].right_light.center.y = msg.armors[i].right_light.center.y;
- armors[i].right_light.bottom.x = msg.armors[i].right_light.bottom.x;
- armors[i].right_light.bottom.y = msg.armors[i].right_light.bottom.y;
- armors[i].right_light.size.width = msg.armors[i].right_light.size.x;
- armors[i].right_light.size.height = msg.armors[i].right_light.size.y;
- armors[i].right_light.length = msg.armors[i].right_light.length;
- armors[i].right_light.width = msg.armors[i].right_light.width;
- armors[i].right_light.tilt_angle = msg.armors[i].right_light.tilt_angle;
- armors[i].right_light.angle = msg.armors[i].right_light.angle;
-
- armors[i].center.x = msg.armors[i].center.x;
- armors[i].center.y = msg.armors[i].center.y;
-
- armors[i].points2d.resize(msg.armors[i].points2d.size());
- for(size_t j = 0; j < msg.armors[i].points2d.size(); j++)
+ std::vector armors;
+ armors.resize(msg.armors.size());
+ for (size_t i = 0; i < msg.armors.size(); i++)
{
- armors[i].points2d[j].x = msg.armors[i].points2d[j].x;
- armors[i].points2d[j].y = msg.armors[i].points2d[j].y;
+ armors[i].left_light.color = msg.armors[i].left_light.color;
+ armors[i].left_light.top.x = msg.armors[i].left_light.top.x;
+ armors[i].left_light.top.y = msg.armors[i].left_light.top.y;
+ armors[i].left_light.center.x = msg.armors[i].left_light.center.x;
+ armors[i].left_light.center.y = msg.armors[i].left_light.center.y;
+ armors[i].left_light.bottom.x = msg.armors[i].left_light.bottom.x;
+ armors[i].left_light.bottom.y = msg.armors[i].left_light.bottom.y;
+ armors[i].left_light.size.width = msg.armors[i].left_light.size.x;
+ armors[i].left_light.size.height = msg.armors[i].left_light.size.y;
+ armors[i].left_light.length = msg.armors[i].left_light.length;
+ armors[i].left_light.width = msg.armors[i].left_light.width;
+ armors[i].left_light.tilt_angle = msg.armors[i].left_light.tilt_angle;
+ armors[i].left_light.angle = msg.armors[i].left_light.angle;
+
+ armors[i].right_light.color = msg.armors[i].right_light.color;
+ armors[i].right_light.top.x = msg.armors[i].right_light.top.x;
+ armors[i].right_light.top.y = msg.armors[i].right_light.top.y;
+ armors[i].right_light.center.x = msg.armors[i].right_light.center.x;
+ armors[i].right_light.center.y = msg.armors[i].right_light.center.y;
+ armors[i].right_light.bottom.x = msg.armors[i].right_light.bottom.x;
+ armors[i].right_light.bottom.y = msg.armors[i].right_light.bottom.y;
+ armors[i].right_light.size.width = msg.armors[i].right_light.size.x;
+ armors[i].right_light.size.height = msg.armors[i].right_light.size.y;
+ armors[i].right_light.length = msg.armors[i].right_light.length;
+ armors[i].right_light.width = msg.armors[i].right_light.width;
+ armors[i].right_light.tilt_angle = msg.armors[i].right_light.tilt_angle;
+ armors[i].right_light.angle = msg.armors[i].right_light.angle;
+
+ armors[i].center.x = msg.armors[i].center.x;
+ armors[i].center.y = msg.armors[i].center.y;
+
+ armors[i].points2d.resize(msg.armors[i].points2d.size());
+ for (size_t j = 0; j < msg.armors[i].points2d.size(); j++)
+ {
+ armors[i].points2d[j].x = msg.armors[i].points2d[j].x;
+ armors[i].points2d[j].y = msg.armors[i].points2d[j].y;
+ }
+
+ armors[i].number_img
+ = cv_bridge::toCvCopy(msg.armors[i].number_img, sensor_msgs::image_encodings::MONO8)->image;
+ armors[i].number = msg.armors[i].number;
+ armors[i].idx = msg.armors[i].idx;
+ armors[i].similarity = msg.armors[i].similarity;
+ armors[i].confidence = msg.armors[i].confidence;
+ armors[i].classification_result = msg.armors[i].classification_result;
+ armors[i].armor_type = ArmorType(msg.armors[i].armor_type.data);
}
- armors[i].number_img
- = cv_bridge::toCvCopy(msg.armors[i].number_img, sensor_msgs::image_encodings::MONO8)->image;
- armors[i].number = msg.armors[i].number;
- armors[i].idx = msg.armors[i].idx;
- armors[i].similarity = msg.armors[i].similarity;
- armors[i].confidence = msg.armors[i].confidence;
- armors[i].classification_result = msg.armors[i].classification_result;
- armors[i].armor_type = ArmorType(msg.armors[i].armor_type.data);
+ return armors;
}
- return armors;
-}
-
} // namespace hnurm
\ No newline at end of file
diff --git a/hnurm_armor/src/main.cpp b/hnurm_armor/src/main.cpp
index ba25f4e546907c06015cf12f62e0e92d61ea7e96..22b25ca001c76f69fc3df8430a7c0ce675ffbc09 100644
--- a/hnurm_armor/src/main.cpp
+++ b/hnurm_armor/src/main.cpp
@@ -5,6 +5,9 @@ int main(int argc, char *argv[])
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
auto node = std::make_shared(options);
- rclcpp::spin(node);
+ // rclcpp::spin(node);
+ rclcpp::executors::MultiThreadedExecutor executor;
+ executor.add_node(node);
+ executor.spin();
rclcpp::shutdown();
}
\ No newline at end of file
diff --git a/hnurm_armor/src/solvePnPBA.cpp b/hnurm_armor/src/solvePnPBA.cpp
deleted file mode 100644
index da98abd6bf77f647393c26a0f96a31afe006a8b2..0000000000000000000000000000000000000000
--- a/hnurm_armor/src/solvePnPBA.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-//
-// Created by Kai Wang on 24-3-14.
-//
-
-#include "solvePnPBA/solvePnPBA.h"
-
-void bundleAdjustmentG2O(
- const VecVector3d &points_3d, const VecVector2d &points_2d, const cv::Mat &K, Sophus::SE3d &pose)
-{
-
- using BlockSolverType = g2o::BlockSolver>;
- using LinearSolverType = g2o::LinearSolverDense;
-
- // GN, LM, DogLeg
- auto solver = new g2o::OptimizationAlgorithmGaussNewton(
- g2o::make_unique(g2o::make_unique()));
- g2o::SparseOptimizer optimizer;
- optimizer.setAlgorithm(solver);
- optimizer.setVerbose(true);
-
- // vertex
- auto *vertex_pose = new VertexPose();// camera vertex_pose
- vertex_pose->setId(0);
- vertex_pose->setEstimate(pose);
- optimizer.addVertex(vertex_pose);
-
- // K
- Eigen::Matrix3d K_eigen;
- cv::cv2eigen(K, K_eigen);
-
- // edges
- int index = 1;
- for (size_t i = 0; i < points_2d.size(); ++i)
- {
- const auto &p2d = points_2d[i];
- const auto &p3d = points_3d[i];
- auto *edge = new EdgeProjection(p3d, K_eigen);
- edge->setId(index);
- edge->setVertex(0, vertex_pose);
- edge->setMeasurement(p2d);
- edge->setInformation(Eigen::Matrix2d::Identity());
- optimizer.addEdge(edge);
- index++;
- }
-
- TIMEIT(optimizer.initializeOptimization(); optimizer.optimize(16););
- pose = vertex_pose->estimate();
-}
-
-bool solvePnPBA(
- cv::InputArray object_points,
- cv::InputArray image_points,
- cv::InputArray cameraMatrix,
- cv::InputArray distCoeffs,
- cv::OutputArray rvec,
- cv::OutputArray tvec,
- bool useExtrinsicGuess)
-{
- (void) distCoeffs;
- (void) useExtrinsicGuess;
-
- cv::Mat obj_mat;
- cv::Mat img_mat;
-
- object_points.getMat().convertTo(obj_mat, CV_64F);
- image_points.getMat().convertTo(img_mat, CV_64F);
-
- std::cout << "obj mat\n"
- << obj_mat << "\n"
- << "sz" << obj_mat.size << "\n";
- std::cout << "img mat\n"
- << img_mat << "\n"
- << "sz" << img_mat.size << "\n";
-
- VecVector3d obj_points_vec;
- VecVector2d img_points_vec;
- for (int i = 0; i < 4; i++)
- {
- obj_points_vec.emplace_back(obj_mat.at(i * 3), obj_mat.at(i * 3 + 1), obj_mat.at(i * 3 + 2));
- img_points_vec.emplace_back(img_mat.at(i * 2), img_mat.at(i * 2 + 1));
- }
-
- Sophus::SE3d pose;
- if (useExtrinsicGuess)
- {
- Eigen::Vector3f init_tvec, init_rvec;
- cv::cv2eigen(tvec.getMat(), init_tvec);
- cv::cv2eigen(rvec.getMat(), init_rvec);
-
- pose.translation() = init_tvec.cast();
- pose.so3() = Sophus::SO3d::exp(init_rvec.cast());
- }
-
- // do bundle adjustment
- bundleAdjustmentG2O(obj_points_vec, img_points_vec, cameraMatrix.getMat(), pose);
-
- // get results
- rvec.create(3, 1, CV_64F);
- tvec.create(3, 1, CV_64F);
- cv::eigen2cv(pose.so3().log(), rvec.getMat());
- cv::eigen2cv(pose.translation(), tvec.getMat());
-
- return true;
-}
diff --git a/hnurm_armor/src/tracker.cpp b/hnurm_armor/src/tracker.cpp
index 92581b2133429fec01e9bbe0512c7675ea7e9945..068fac583223afd250fa52ad26d28afb57110bd7 100755
--- a/hnurm_armor/src/tracker.cpp
+++ b/hnurm_armor/src/tracker.cpp
@@ -14,89 +14,89 @@
namespace hnurm
{
-Tracker::Tracker(rclcpp::Node::SharedPtr node)
-{
- tracker_state = LOST;
- tracked_id = std::string("");
- target_state = Eigen::VectorXd::Zero(9);
-
- max_match_distance_ = node->declare_parameter("processor.tracker.max_match_distance", 0.5);
- max_match_yaw_diff_ = node->declare_parameter("processor.tracker.max_match_yaw_diff", 0.5);
- tracking_threshold_ = node->declare_parameter("processor.tracker.tracking_threshold", 0.0);
- lost_threshold_ = node->declare_parameter("processor.tracker.lost_threshold", 15.0);
-
- measurement = Eigen::Vector4d::Zero();
-}
-
-Tracker::Tracker(double max_match_distance, int tracking_threshold, int lost_threshold)
- : tracker_state(LOST),
- tracked_id(std::string("")),
- target_state(Eigen::VectorXd::Zero(9)),
- max_match_distance_(max_match_distance),
- tracking_threshold_(tracking_threshold),
- lost_threshold_(lost_threshold)
-{
-}
+ Tracker::Tracker(rclcpp::Node::SharedPtr node)
+ {
+ tracker_state = LOST;
+ tracked_id = std::string("");
+ target_state = Eigen::VectorXd::Zero(9);
-void Tracker::init(const std::vector &armors_msg)
-{
- if(armors_msg.empty())
+ max_match_distance_ = node->declare_parameter("processor.tracker.max_match_distance", 0.5);
+ max_match_yaw_diff_ = node->declare_parameter("processor.tracker.max_match_yaw_diff", 0.5);
+ tracking_threshold_ = node->declare_parameter("processor.tracker.tracking_threshold", 0.0);
+ lost_threshold_ = node->declare_parameter("processor.tracker.lost_threshold", 100.0);
+
+ measurement = Eigen::Vector4d::Zero();
+ }
+
+ Tracker::Tracker(double max_match_distance, int tracking_threshold, int lost_threshold)
+ : tracker_state(LOST),
+ tracked_id(std::string("")),
+ target_state(Eigen::VectorXd::Zero(9)),
+ max_match_distance_(max_match_distance),
+ tracking_threshold_(tracking_threshold),
+ lost_threshold_(lost_threshold)
{
- return;
}
- // Simply choose the armor that is closest to image center
- auto min_distance = DBL_MAX;
- tracked_armor = armors_msg[0];
- for(const auto &armor : armors_msg)
+
+ void Tracker::init(const std::vector &armors_msg)
{
- if(armor.distance_to_image_center < min_distance)
+ if (armors_msg.empty())
{
- min_distance = armor.distance_to_image_center;
- tracked_armor = armor;
+ return;
+ }
+ // Simply choose the armor that is closest to image center
+ auto min_distance = DBL_MAX;
+ tracked_armor = armors_msg[0];
+ for (const auto &armor: armors_msg)
+ {
+ if (armor.distance_to_image_center < min_distance)
+ {
+ min_distance = armor.distance_to_image_center;
+ tracked_armor = armor;
+ }
}
- }
-
- initEKF(tracked_armor);
-
- tracked_id = tracked_armor.number;
- tracker_state = DETECTING;
- updateArmorsNum(tracked_armor);
-}
+ initEKF(tracked_armor);
-void Tracker::update(const std::vector &armors_msg)
-{
- // KF predict
- Eigen::VectorXd ekf_prediction = ekf.predict();
- // RCLCPP_DEBUG(rclcpp::get_logger("armor_processor"), "EKF predict");
+ tracked_id = tracked_armor.number;
+ tracker_state = DETECTING;
- bool matched = false;
- // Use KF prediction as default target state if no matched armor is found
- target_state = ekf_prediction;
+ updateArmorsNum(tracked_armor);
+ }
- if (!armors_msg.empty())
+ void Tracker::update(const std::vector &armors_msg)
{
- auto min_position_diff = DBL_MAX;
- auto yaw_diff = DBL_MAX;
+ // KF predict
+ Eigen::VectorXd ekf_prediction = ekf.predict();
+ // RCLCPP_DEBUG(rclcpp::get_logger("armor_processor"), "EKF predict");
+
+ bool matched = false;
+ // Use KF prediction as default target state if no matched armor is found
+ target_state = ekf_prediction;
- auto predicted_position = getArmorPositionFromState(ekf_prediction);
- for (const auto &armor : armors_msg)
+ if (!armors_msg.empty())
{
- // 仅考虑相同ID的装甲板
- if (armor.number == tracked_id)
+ auto min_position_diff = DBL_MAX;
+ auto yaw_diff = DBL_MAX;
+
+ auto predicted_position = getArmorPositionFromState(ekf_prediction);
+ for (const auto &armor: armors_msg)
{
- auto p = armor.position;
- // 当前装甲板位置与跟踪装甲板的预测位置的差异
- double position_diff = (predicted_position - p).norm();
- if (position_diff < min_position_diff)
+ // 仅考虑相同ID的装甲板
+ if (armor.number == tracked_id)
{
- min_position_diff = position_diff;
- // 计算当前装甲板的姿态与EKF预测的姿态之间的差异
- yaw_diff = abs(orientationToYaw(armor.rotation) - ekf_prediction(6));
- tracked_armor = armor;
+ auto p = armor.position;
+ // 当前装甲板位置与跟踪装甲板的预测位置的差异
+ double position_diff = (predicted_position - p).norm();
+ if (position_diff < min_position_diff)
+ {
+ min_position_diff = position_diff;
+ // 计算当前装甲板的姿态与EKF预测的姿态之间的差异
+ yaw_diff = abs(orientationToYaw(armor.rotation) - ekf_prediction(6));
+ tracked_armor = armor;
+ }
}
}
- }
// 保存差异信息
info_position_diff = min_position_diff;
info_yaw_diff = yaw_diff;
@@ -115,152 +115,192 @@ void Tracker::update(const std::vector &armors_msg)
{
// 处理装甲板跳变情况
handleArmorJump(tracked_armor);
+ if (tracker_state == TRACKING)
+ {
+ tracker_state = JUMP;
+ }
}
else
{
// 装甲板未匹配,追踪状态设置为丢失
tracker_state = LOST;
}
- }
+ }
- // Prevent radius from spreading
- target_state(8) = CLAMP(target_state(8), 0.14, 0.33);
- another_r = CLAMP(another_r, 0.14, 0.33);
- // Tracking state machine
- if(tracker_state == DETECTING)
- {
- if(matched)
+ // Prevent radius from spreading
+ target_state(8) = CLAMP(target_state(8), 0.14, 0.33);
+ another_r = CLAMP(another_r, 0.14, 0.33);
+ // Tracking state machine
+ if (tracker_state == DETECTING)
{
- detect_count_++;
- if(detect_count_ > tracking_threshold_)
+ if (matched)
+ {
+ detect_count_++;
+ if (detect_count_ > tracking_threshold_)
+ {
+ detect_count_ = 0;
+ tracker_state = TRACKING;
+ }
+ }
+ else
{
detect_count_ = 0;
- tracker_state = TRACKING;
+ tracker_state = LOST;
}
}
- else
+ else if (tracker_state == TRACKING || tracker_state == JUMP)
{
- detect_count_ = 0;
- tracker_state = LOST;
- }
- }
- else if(tracker_state == TRACKING)
- {
- if(!matched)
- {
- tracker_state = TEMP_LOST;
- lost_count_++;
- }
- }
- else if(tracker_state == TEMP_LOST)
- {
- if(!matched)
- {
- lost_count_++;
- if(lost_count_ > lost_threshold_)
+ if (!matched && tracker_state != JUMP)
{
- lost_count_ = 0;
- tracker_state = LOST;
+ tracker_state = TEMP_LOST;
+ lost_count_++;
+ }
+ if (matched && tracker_state == JUMP)
+ {
+ tracker_state = TRACKING;
+ }
+ if (!matched && tracker_state == JUMP)
+ {
+ lost_count_++;
+ if (lost_count_ > lost_threshold_)
+ {
+ lost_count_ = 0;
+ tracker_state = LOST;
+ }
}
}
- else
+ else if (tracker_state == TEMP_LOST)
{
- tracker_state = TRACKING;
- lost_count_ = 0;
+ if (!matched)
+ {
+ lost_count_++;
+ if (lost_count_ > lost_threshold_)
+ {
+ lost_count_ = 0;
+ tracker_state = LOST;
+ }
+ }
+ else
+ {
+ tracker_state = TRACKING;
+ lost_count_ = 0;
+ }
}
}
-}
-void Tracker::initEKF(const Armor &a)
-{
- double xa = a.position(0);
- double ya = a.position(1);
- double za = a.position(2);
- last_yaw_ = 0;
- // todo
- double yaw = orientationToYaw(a.rotation);
-
- // Set initial position at 0.2m behind the target
- target_state = Eigen::VectorXd::Zero(9);
- double r = 0.20;
- double xc = xa + r * cos(yaw);
- double yc = ya + r * sin(yaw);
- dz = 0, another_r = r;
- target_state << xc, 0, yc, 0, za, 0, yaw, 0, r;
-
- ekf.setState(target_state);
- // RCLCPP_DEBUG(rclcpp::get_logger("armor_processor"), "Init EKF!");
-}
-
-void Tracker::handleArmorJump(const Armor &a)
-{
- // todo
- double yaw = orientationToYaw(a.rotation);
- target_state(6) = yaw;
+ void Tracker::initEKF(const Armor &a)
+ {
+ double xa = a.position(0);
+ double ya = a.position(1);
+ double za = a.position(2);
+ last_yaw_ = 0;
+ // todo
+ double yaw = orientationToYaw(a.rotation);
- updateArmorsNum(a);
+ // Set initial position at 0.2m behind the target
+ target_state = Eigen::VectorXd::Zero(9);
+ double r = 0.20;
+ double xc = xa + r * cos(yaw);
+ double yc = ya + r * sin(yaw);
+ dz = 0, another_r = r;
+ target_state << xc, 0, yc, 0, za, 0, yaw, 0, r;
- if(tracked_armors_num == ArmorsNum::NORMAL_4)
- {
- dz = target_state(4) - a.position(2);
- target_state(4) = a.position(2);
- std::swap(target_state(8), another_r);
+ ekf.setState(target_state);
+ // RCLCPP_DEBUG(rclcpp::get_logger("armor_processor"), "Init EKF!");
}
- auto p = a.position;
+ void Tracker::handleArmorJump(const Armor &a)
+ {
+ // todo
+ double yaw = orientationToYaw(a.rotation);
+ target_state(6) = yaw;
- Eigen::Vector3d infer_p = getArmorPositionFromState(target_state);
+ updateArmorsNum(a);
- if((p - infer_p).norm() > max_match_distance_)
- {
- double r = target_state(8);
- target_state(0) = p(0) + r * cos(yaw); // xc
- target_state(1) = 0; // vxc
- target_state(2) = p(1) + r * sin(yaw); // yc
- target_state(3) = 0; // vyc
- target_state(4) = p(2); // za
- target_state(5) = 0; // vza
- // CLOG(WARNING, "process") << "Reset state!";
- RCLCPP_WARN(rclcpp::get_logger("armor_processor"), "Reset state!");
- }
+ if (tracked_armors_num == ArmorsNum::NORMAL_4)
+ {
+ dz = target_state(4) - a.position(2);
+ target_state(4) = a.position(2);
+ std::swap(target_state(8), another_r);
+ }
- ekf.setState(target_state);
-}
+ auto p = a.position;
-// todo Get armor yaw
-double Tracker::orientationToYaw(const Eigen::Matrix3d &R)
-{
- double yaw = atan2(R(1, 1), R(0, 1));
- armor_pitch=atan2(R(2,2),R(1,2))/CV_PI*180;
- yaw = last_yaw_ + angles::shortest_angular_distance(last_yaw_, yaw);
- last_yaw_ = yaw;
- return yaw;
-}
-
-Eigen::Vector3d Tracker::getArmorPositionFromState(const Eigen::VectorXd &x)
-{
- // Calculate predicted position of the current armor
- double xc = x(0), yc = x(2), zc = x(4);
- double yaw = x(6), r = x(8);
- double xa = xc - r * cos(yaw);
- double ya = yc - r * sin(yaw);
- return {xa, ya, zc};
-}
-
-void Tracker::updateArmorsNum(const Armor &armor)
-{
- if(armor.armor_type == LARGE && (tracked_id == "3" || tracked_id == "4" || tracked_id == "5"))
+ Eigen::Vector3d infer_p = getArmorPositionFromState(target_state);
+
+ if ((p - infer_p).norm() > max_match_distance_)
+ {
+ double r = target_state(8);
+ target_state(0) = p(0) + r * cos(yaw); // xc
+ target_state(1) = 0; // vxc
+ target_state(2) = p(1) + r * sin(yaw); // yc
+ target_state(3) = 0; // vyc
+ target_state(4) = p(2); // za
+ target_state(5) = 0; // vza
+ // CLOG(WARNING, "process") << "Reset state!";
+ RCLCPP_WARN(rclcpp::get_logger("armor_processor"), "Reset state!");
+ }
+
+ ekf.setState(target_state);
+ }
+
+ double Tracker::orientationToYaw(const Eigen::Matrix3d &R)
{
- tracked_armors_num = ArmorsNum::BALANCE_2;
+ double sy = sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0));
+ Eigen::Vector3d eulerAngles;
+ bool singular = sy < 1e-6; // If
+
+ if (!singular)
+ {
+ eulerAngles[0] = atan2(R(2, 1), R(2, 2)); // X-axis rotation
+ eulerAngles[1] = atan2(-R(2, 0), sy); // Y-axis rotation
+ eulerAngles[2] = atan2(R(1, 0), R(0, 0)); // Z-axis rotation
+ }
+ else
+ {
+ eulerAngles[0] = atan2(-R(1, 2), R(1, 1)); // X-axis rotation
+ eulerAngles[1] = atan2(-R(2, 0), sy); // Y-axis rotation
+ eulerAngles[2] = 0; // Z-axis rotation
+ }
+ double yaw = eulerAngles[2] + CV_PI / 2;
+ armor_pitch = eulerAngles[0] / CV_PI * 180;
+ armor_yaw = eulerAngles[2] / CV_PI * 180;
+// double yaw = atan2(R(1, 1), R(0, 1));
+// armor_pitch=atan2(R(2,2),R(1,2))/CV_PI*180;
+// armor_yaw=yaw/CV_PI*180;
+ yaw = last_yaw_ + angles::shortest_angular_distance(last_yaw_, yaw);
+ last_yaw_ = yaw;
+ return yaw;
}
- else if(tracked_id == "outpost")
+
+ Eigen::Vector3d Tracker::getArmorPositionFromState(const Eigen::VectorXd &x)
{
- tracked_armors_num = ArmorsNum::OUTPOST_3;
+ // Calculate predicted position of the current armor
+ double xc = x(0), yc = x(2), zc = x(4);
+ double yaw = x(6), r = x(8);
+ double xa = xc - r * cos(yaw);
+ double ya = yc - r * sin(yaw);
+ return {xa, ya, zc};
}
- else
+
+ void Tracker::updateArmorsNum(const Armor &armor)
{
- tracked_armors_num = ArmorsNum::NORMAL_4;
+ if (armor.armor_type == LARGE && (tracked_id == "3" || tracked_id == "4" || tracked_id == "5"))
+ {
+ tracked_armors_num = ArmorsNum::BALANCE_2;
+ }
+ else if (tracked_id == "outpost")
+ {
+ tracked_armors_num = ArmorsNum::OUTPOST_3;
+ }
+ else if (tracked_id == "base")
+ {
+ tracked_armors_num = ArmorsNum::BASE_1;
+ }
+ else
+ {
+ tracked_armors_num = ArmorsNum::NORMAL_4;
+ }
}
-}
} // namespace hnurm
diff --git a/hnurm_bringup/CMakeLists.txt b/hnurm_bringup/CMakeLists.txt
index 4207d86169029ad84b7515553b5e80e7801823eb..35873f6834c98cab027e4dba578c74aa15ab0c45 100644
--- a/hnurm_bringup/CMakeLists.txt
+++ b/hnurm_bringup/CMakeLists.txt
@@ -9,17 +9,22 @@ endif ()
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()
-ament_auto_add_executable(hnurm_bringup_node
- src/bringup_node.cpp
+ament_auto_add_executable(sentry_manage_node
+ src/sentry_manage_node.cpp
)
-install(TARGETS
- hnurm_bringup_node
- DESTINATION lib/${PROJECT_NAME}
+ament_auto_add_executable(compose_node
+ src/compose_node.cpp
)
-
-install(DIRECTORY launch params
- DESTINATION share/${PROJECT_NAME}
+ament_target_dependencies(compose_node
+ hnurm_armor
+ hnurm_uart
+ hnurm_detect
+ hnurm_camera
)
-ament_package()
+ament_auto_package(
+ INSTALL_TO_SHARE
+ params
+ launch
+)
diff --git a/hnurm_bringup/include/hnurm_bringup/bringup_node.h b/hnurm_bringup/include/hnurm_bringup/sentry_manage_node.h
similarity index 44%
rename from hnurm_bringup/include/hnurm_bringup/bringup_node.h
rename to hnurm_bringup/include/hnurm_bringup/sentry_manage_node.h
index b1240d72e102075533db92ac7c3ae06172e226e5..f316fee307a2331c58ea1b2a4f76c7d7946cafd6 100644
--- a/hnurm_bringup/include/hnurm_bringup/bringup_node.h
+++ b/hnurm_bringup/include/hnurm_bringup/sentry_manage_node.h
@@ -8,28 +8,28 @@
#include
#include
-#include "hnurm_uart/msg/vision_send_data.hpp"
+#include "hnurm_interfaces/msg/vision_send_data.hpp"
namespace hnurm
{
-class BringUpNode : public rclcpp::Node
+class SentryManageNode : public rclcpp::Node
{
public:
- explicit BringUpNode(const rclcpp::NodeOptions &options);
+ explicit SentryManageNode(const rclcpp::NodeOptions &options);
void init_params();
- void master_res_callback(hnurm_uart::msg::VisionSendData::ConstSharedPtr msg);
- void slave_res_callback(hnurm_uart::msg::VisionSendData::ConstSharedPtr msg);
+ void master_res_callback(hnurm_interfaces::msg::VisionSendData::ConstSharedPtr msg);
+ void slave_res_callback(hnurm_interfaces::msg::VisionSendData::ConstSharedPtr msg);
private:
// subs
- rclcpp::Subscription::SharedPtr master_res_sub_;
- rclcpp::Subscription::SharedPtr slave_res_sub_;
+ rclcpp::Subscription::SharedPtr master_res_sub_;
+ rclcpp::Subscription::SharedPtr slave_res_sub_;
// pubs
- rclcpp::Publisher::SharedPtr master_res_pub_;
- rclcpp::Publisher::SharedPtr slave_res_pub_;
+ rclcpp::Publisher::SharedPtr master_res_pub_;
+ rclcpp::Publisher::SharedPtr slave_res_pub_;
// topics
std::string master_res_sub_topic_;
@@ -40,7 +40,7 @@ private:
// buffers
std::mutex master_res_buffer_mutex_;
- std::deque master_res_buffer_;
+ std::deque master_res_buffer_;
// params
bool use_cross_fire = false;
diff --git a/hnurm_bringup/launch/compose.launch.py b/hnurm_bringup/launch/compose.launch.py
new file mode 100644
index 0000000000000000000000000000000000000000..4ce6d7d70d15bd01ec2e3fa2ae9e91b11d6b3d6b
--- /dev/null
+++ b/hnurm_bringup/launch/compose.launch.py
@@ -0,0 +1,60 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ hnurm_bringup_dir = get_package_share_directory('hnurm_bringup')
+
+ # getting all params
+ hnurm_uart_dir = get_package_share_directory('hnurm_uart')
+ hnurm_camera_dir = get_package_share_directory('hnurm_camera')
+ hnurm_detect_dir = get_package_share_directory('hnurm_detect')
+ hnurm_armor_dir = get_package_share_directory('hnurm_armor')
+
+ params_file_uart = LaunchConfiguration('uart_params_file')
+ params_file_camera = LaunchConfiguration('camera_params_file')
+ params_file_detect = LaunchConfiguration('detect_params_file')
+ params_file_armor = LaunchConfiguration('armor_params_file')
+
+ hnurm_bringup_ld = LaunchDescription([
+ DeclareLaunchArgument(
+ 'uart_params_file',
+ default_value=os.path.join(hnurm_uart_dir, 'params', 'default.yaml'),
+ description='uart params file'
+ ),
+
+ DeclareLaunchArgument(
+ 'camera_params_file',
+ default_value=os.path.join(hnurm_camera_dir, 'params', 'default.yaml'),
+ description='camera params file'
+ ),
+
+ DeclareLaunchArgument(
+ 'detect_params_file',
+ default_value=os.path.join(hnurm_detect_dir, 'params', 'default.yaml'),
+ description='detect params file'
+ ),
+
+ DeclareLaunchArgument(
+ 'armor_params_file',
+ default_value=os.path.join(hnurm_armor_dir, 'params', 'default.yaml'),
+ description='armor params file'
+ ),
+
+ Node(
+ package='hnurm_bringup',
+ executable='compose_node',
+ output='screen',
+ parameters=[params_file_uart, params_file_camera, params_file_detect, params_file_armor]
+ ),
+ ])
+
+ # now return the launch description
+ return LaunchDescription([
+ hnurm_bringup_ld
+ ])
diff --git a/hnurm_bringup/launch/vision.uart.cam.launch.py b/hnurm_bringup/launch/vision.uart.cam.launch.py
new file mode 100644
index 0000000000000000000000000000000000000000..961f254b556d471ddb5d35472c5e9e306a383d69
--- /dev/null
+++ b/hnurm_bringup/launch/vision.uart.cam.launch.py
@@ -0,0 +1,61 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ # firstly getting the uart node
+ # in this node, we need to solve control_id
+ hnurm_uart_dir = get_package_share_directory('hnurm_uart')
+ params_file_uart = LaunchConfiguration('uart_params_file')
+ control_id = LaunchConfiguration('control_id')
+
+ hnurm_uart_ld = LaunchDescription([
+ DeclareLaunchArgument(
+ 'uart_params_file',
+ default_value=os.path.join(hnurm_uart_dir, 'params', 'default.yaml'),
+ description='uart params file'
+ ),
+ DeclareLaunchArgument(
+ 'control_id',
+ default_value="1.0",
+ description='control id'
+ ),
+ Node(
+ package='hnurm_uart',
+ executable='hnurm_uart_node',
+ output='screen',
+ parameters=[params_file_uart, {'control_id': control_id}]
+ ),
+ ])
+
+ # getting the camera node
+ # should tell which camera to open
+ # so camera_id param should be rewritten
+ hnurm_camera_dir = get_package_share_directory('hnurm_camera')
+ params_file_camera = LaunchConfiguration('camera_params_file')
+
+ hnurm_camera_ld = LaunchDescription([
+ DeclareLaunchArgument(
+ 'camera_params_file',
+ default_value=os.path.join(hnurm_camera_dir, 'params', 'default.yaml'),
+ description='camera params file'
+ ),
+ Node(
+ package='hnurm_camera',
+ executable='hnurm_camera_node',
+ output='screen',
+ parameters=[params_file_camera]
+ )
+ ])
+
+ # now return the launch description
+ return LaunchDescription([
+ hnurm_uart_ld,
+ hnurm_camera_ld,
+ ])
diff --git a/hnurm_bringup/package.xml b/hnurm_bringup/package.xml
index 9564c6ed4ddc30d5ff1fdcd762fa46ff71f77f58..6dd8ae3a5defd45f9dada9a6d6baf299ba154a5c 100644
--- a/hnurm_bringup/package.xml
+++ b/hnurm_bringup/package.xml
@@ -15,8 +15,12 @@
tf2
tf2_eigen
angles
- hnurm_uart
+ hnurm_interfaces
+ hnurm_detect
+ hnurm_camera
+ hnurm_uart
+ hnurm_armor
ament_lint_auto
ament_lint_common
diff --git a/hnurm_bringup/src/compose_node.cpp b/hnurm_bringup/src/compose_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eef6dd47782d1de81af61640d3d4c617ad767637
--- /dev/null
+++ b/hnurm_bringup/src/compose_node.cpp
@@ -0,0 +1,36 @@
+//
+// Created by Kai Wang on 24-5-22.
+//
+#include
+#include
+#include
+#include
+
+using namespace std::chrono_literals;
+
+int main(int argc, char *argv[])
+{
+ rclcpp::init(argc, argv);
+
+ rclcpp::NodeOptions options;
+ options.use_intra_process_comms(true);
+
+ auto uart_node = std::make_shared(options);
+ auto camera_node = std::make_shared(options);
+ auto detect_node = std::make_shared(options);
+ auto armor_node = std::make_shared(options);
+
+ rclcpp::executors::MultiThreadedExecutor executor;
+ executor.add_node(uart_node);
+ executor.add_node(camera_node);
+ executor.add_node(detect_node);
+ executor.add_node(armor_node);
+
+ uart_node->run();
+ camera_node->run();
+
+ executor.spin();
+
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/hnurm_bringup/src/bringup_node.cpp b/hnurm_bringup/src/sentry_manage_node.cpp
similarity index 58%
rename from hnurm_bringup/src/bringup_node.cpp
rename to hnurm_bringup/src/sentry_manage_node.cpp
index 0f9495d4a354e3f157c0e40785da7d804b932ecf..5c9f727c6bf7d0fa955287e535cde1a47206c2fc 100644
--- a/hnurm_bringup/src/bringup_node.cpp
+++ b/hnurm_bringup/src/sentry_manage_node.cpp
@@ -2,28 +2,28 @@
// Created by rm on 24-4-8.
//
-#include "hnurm_bringup/bringup_node.h"
+#include "hnurm_bringup/sentry_manage_node.h"
#include
namespace hnurm
{
-BringUpNode::BringUpNode(const rclcpp::NodeOptions &options) : rclcpp::Node("bringup_node", options)
+SentryManageNode::SentryManageNode(const rclcpp::NodeOptions &options) : rclcpp::Node("SentryManageNode", options)
{
init_params();
- master_res_sub_ = this->create_subscription(
- master_res_sub_topic_, 10, std::bind(&BringUpNode::master_res_callback, this, std::placeholders::_1)
+ master_res_sub_ = this->create_subscription(
+ master_res_sub_topic_, 10, std::bind(&SentryManageNode::master_res_callback, this, std::placeholders::_1)
);
- slave_res_sub_ = this->create_subscription(
- slave_res_sub_topic_, 10, std::bind(&BringUpNode::slave_res_callback, this, std::placeholders::_1)
+ slave_res_sub_ = this->create_subscription(
+ slave_res_sub_topic_, 10, std::bind(&SentryManageNode::slave_res_callback, this, std::placeholders::_1)
);
- master_res_pub_ = this->create_publisher(master_res_pub_topic_, 10);
- slave_res_pub_ = this->create_publisher(slave_res_pub_topic_, 10);
+ master_res_pub_ = this->create_publisher(master_res_pub_topic_, 10);
+ slave_res_pub_ = this->create_publisher(slave_res_pub_topic_, 10);
}
-void BringUpNode::init_params()
+void SentryManageNode::init_params()
{
master_res_sub_topic_ = this->declare_parameter("/master_res", "/left/vision_send_res");
slave_res_sub_topic_ = this->declare_parameter("/slave_res", "/right/vision_send_res");
@@ -35,11 +35,11 @@ void BringUpNode::init_params()
master_to_slave_offset = this->declare_parameter("/master_to_slave_offset", 0.335);
}
-void BringUpNode::master_res_callback(hnurm_uart::msg::VisionSendData::ConstSharedPtr msg)
+void SentryManageNode::master_res_callback(hnurm_interfaces::msg::VisionSendData::ConstSharedPtr msg)
{
if(use_cross_fire)
{
- if(msg->target_state.data == hnurm_uart::msg::TargetState::FIRE)
+ if(msg->target_state.data == hnurm_interfaces::msg::TargetState::FIRE)
master_res_buffer_.push_front(*msg);
else
master_res_buffer_.clear();
@@ -47,7 +47,7 @@ void BringUpNode::master_res_callback(hnurm_uart::msg::VisionSendData::ConstShar
master_res_pub_->publish(*msg);
}
-void BringUpNode::slave_res_callback(hnurm_uart::msg::VisionSendData::ConstSharedPtr msg)
+void SentryManageNode::slave_res_callback(hnurm_interfaces::msg::VisionSendData::ConstSharedPtr msg)
{
if(!use_cross_fire)
{
@@ -55,14 +55,14 @@ void BringUpNode::slave_res_callback(hnurm_uart::msg::VisionSendData::ConstShare
return;
}
- if(msg->target_state.data == hnurm_uart::msg::TargetState::FIRE)
+ if(msg->target_state.data == hnurm_interfaces::msg::TargetState::FIRE)
{
// this means slave head has target
slave_res_pub_->publish(*msg);
return;
}
- if(msg->target_state.data == hnurm_uart::msg::TargetState::CONVERGING)
+ if(msg->target_state.data == hnurm_interfaces::msg::TargetState::CONVERGING)
{
// this means slave head has target
slave_res_pub_->publish(*msg);
@@ -77,7 +77,7 @@ void BringUpNode::slave_res_callback(hnurm_uart::msg::VisionSendData::ConstShare
}
// cross fire
- auto master_res = master_res_buffer_.front();
+ auto master_res = master_res_buffer_.front();
// auto master_time = rclcpp::Time(master_res.header.stamp);
// auto slave_time = rclcpp::Time(msg->header.stamp);
// auto diff = (master_time - slave_time).seconds();
@@ -93,15 +93,15 @@ void BringUpNode::slave_res_callback(hnurm_uart::msg::VisionSendData::ConstShare
// get target
const auto &d = master_to_slave_offset;
const auto &r = master_res.target_distance;
- const auto phi = angles::from_degrees(master_res.yaw);
+ const auto phi = angles::from_degrees(master_res.yaw);
// theta is target
- auto theta = static_cast(atan(r * sin(phi) / (r * cos(phi) - d)));
- auto total_yaw = angles::from_degrees(msg->yaw);
+ auto theta = static_cast(atan(r * sin(phi) / (r * cos(phi) - d)));
+ auto total_yaw = angles::from_degrees(msg->yaw);
// get angular distance in degs
auto da = angles::to_degrees(angles::shortest_angular_distance(total_yaw, theta));
// calculate final target in degs
- auto yaw_target = msg->yaw + da +12 ;
- bool status_ = true;
+ auto yaw_target = msg->yaw + da + 12;
+ bool status_ = true;
// if(master_res.yaw > 180)
// {
@@ -112,30 +112,27 @@ void BringUpNode::slave_res_callback(hnurm_uart::msg::VisionSendData::ConstShare
yaw_target = yaw_target - 195;
}
+ if(master_res.yaw > 80 && master_res.yaw < 110)
+ {
+ status_ = false;
+ }
- if(master_res.yaw >80 && master_res.yaw <110)
- {
- status_ =false;
- }
-
- if(status_){
- hnurm_uart::msg::VisionSendData res;
+ if(status_)
+ {
+ hnurm_interfaces::msg::VisionSendData res;
res.header = msg->header;
- res.target_state.data = hnurm_uart::msg::TargetState::CONVERGING;
+ res.target_state.data = hnurm_interfaces::msg::TargetState::CONVERGING;
res.target_type = master_res.target_type;
res.pitch = master_res.pitch;
res.yaw = yaw_target;
res.control_id = 1.000;
slave_res_pub_->publish(res);
master_res_buffer_.pop_front();
- if(master_res_buffer_.size()>10000)
+ if(master_res_buffer_.size() > 10'000)
{
master_res_buffer_.clear();
}
}
-
-
-
}
} // namespace hnurm
@@ -143,7 +140,7 @@ int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
- auto node = std::make_shared(options);
+ auto node = std::make_shared(options);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
diff --git a/hnurm_camera/CMakeLists.txt b/hnurm_camera/CMakeLists.txt
index 933ef297bfd7f9ccf401379097bdd4510dbd46a7..37a1fd082377ecca38fc7db1dd3dd032d53f1b54 100644
--- a/hnurm_camera/CMakeLists.txt
+++ b/hnurm_camera/CMakeLists.txt
@@ -1,8 +1,7 @@
cmake_minimum_required(VERSION 3.22)
project(hnurm_camera)
-
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
if (${CMAKE_SYSTEM} MATCHES "Linux")
file(GLOB SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/linux/*)
@@ -10,48 +9,17 @@ elseif (${CMAKE_SYSTEM} MATCHES "Darwin")
file(GLOB SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/darwin/*)
endif ()
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(cv_bridge REQUIRED)
-find_package(sensor_msgs REQUIRED)
-
-find_package(OpenCV REQUIRED)
-
-set(dependencies
- rclcpp
- OpenCV
- cv_bridge
- sensor_msgs
-)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
-add_library(hnurm_camera SHARED src/Camera.cpp)
-target_include_directories(hnurm_camera PRIVATE
- $
- $
-)
-ament_target_dependencies(hnurm_camera ${dependencies})
+ament_auto_add_library(hnurm_camera SHARED src/Camera.cpp src/camera_node.cpp)
target_link_libraries(hnurm_camera ${SDK_PATH})
-add_executable(${PROJECT_NAME}_node src/camera_node.cpp src/main.cpp)
-target_include_directories(${PROJECT_NAME}_node PRIVATE
- $
- $
-)
-ament_target_dependencies(${PROJECT_NAME}_node ${dependencies})
-target_link_libraries(${PROJECT_NAME}_node hnurm_camera ${SDK_PATH})
+ament_auto_add_executable(hnurm_camera_node src/main.cpp)
+target_link_libraries(hnurm_camera_node hnurm_camera)
-
-install(TARGETS hnurm_camera
- DESTINATION lib
-)
-install(FILES ${SDK_PATH}
- DESTINATION lib
-)
-install(TARGETS ${PROJECT_NAME}_node
- DESTINATION lib/${PROJECT_NAME}
+ament_auto_package(
+ INSTALL_TO_SHARE
+ launch
+ params
)
-install(DIRECTORY launch params
- DESTINATION share/${PROJECT_NAME}
-)
-
-ament_package()
diff --git a/hnurm_camera/CameraCalibration/CMakeLists.txt b/hnurm_camera/CameraCalibration/CMakeLists.txt
deleted file mode 100755
index c157a739361fec3e6755fe540411ca9439286aaf..0000000000000000000000000000000000000000
--- a/hnurm_camera/CameraCalibration/CMakeLists.txt
+++ /dev/null
@@ -1,27 +0,0 @@
-cmake_minimum_required(VERSION 3.16)
-
-project(Camera_Calibration)
-set(CMAKE_CXX_STANDARD 17)
-set(ROOT ${CMAKE_CURRENT_SOURCE_DIR}/../..)
-find_package(OpenCV REQUIRED)
-
-include_directories(${OpenCV_INCLUDE_DIR}
- include
- )
-
-file(GLOB_RECURSE src_lists
- src/*
- )
-
-add_executable(Camera_Calibration camera_calibration.cpp ${src_lists})
-
-
-#dynamic link libraries
-
-target_link_libraries(Camera_Calibration ${OpenCV_LIBS}
- ${ROOT}/hnurm_camera/lib/linux/libFormatConversion.so
- ${ROOT}/hnurm_camera/lib/linux/libMediaProcess.so
- ${ROOT}/hnurm_camera/lib/linux/libMvCameraControl.so
- ${ROOT}/hnurm_camera/lib/linux/libMVRender.so
- ${ROOT}/hnurm_camera/lib/linux/libMvUsb3vTL.so
- )
\ No newline at end of file
diff --git a/hnurm_camera/CameraCalibration/CameraParam.yaml b/hnurm_camera/CameraCalibration/CameraParam.yaml
deleted file mode 100755
index 911bcb71d2b8810826d133db4164a83a4d6e6438..0000000000000000000000000000000000000000
--- a/hnurm_camera/CameraCalibration/CameraParam.yaml
+++ /dev/null
@@ -1,66 +0,0 @@
-%YAML:1.0
-
----
-# 图像宽度:步进16
-nWidthValue: 1280
-
-# 图像高度:步进2
-nHeightValue: 720
-
-# 水平偏移
-nOffsetXValue: 0
-
-# 垂直偏移
-nOffsetYValue: 152
-
-# 水平镜像
-bSetBoolValue5: 0
-
-# 垂直镜像
-bSetBoolValue1: 0
-
-# 像素格式
-PixelFormat: 0x02180015
-
-# 水平合并
-nBinningHorizontal: 1
-
-# 垂直合并
-nBinningVertical: 1
-
-# 水平下采样
-nDecimationHorizontal: 1
-
-# 垂直下采样
-nDecimationVertical: 1
-
-# 采集触发帧率
-nAcquisitionBurstFrameCountValue: 1
-
-# 设置采集帧率
-fFPSValue: 404.0000
-
-# 设置使能采集帧率控制
-nSetBoolValue3: 1
-
-# 曝光时间
-fExposureTime: 30000.0000
-
-# 增益
-fGainValue: 10.0000
-
-# 自动增益
-nGainAuto: 0
-
-# 黑电平
-nBlackLevelValue: 30
-
-# 黑电平使能
-bSetBoolValue2: 1
-
-# 伽马校正
-fGammaValue: 0.7
-
-# 伽马校正使能
-bSetBoolValue4: 1
-...
\ No newline at end of file
diff --git a/hnurm_camera/CameraCalibration/camera_calibration.cpp b/hnurm_camera/CameraCalibration/camera_calibration.cpp
deleted file mode 100755
index 41a9b835f7cb258ea03ef2e1fe7ed242a7737db6..0000000000000000000000000000000000000000
--- a/hnurm_camera/CameraCalibration/camera_calibration.cpp
+++ /dev/null
@@ -1,722 +0,0 @@
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include "Camera.h"
-using namespace cv;
-using namespace std;
-using namespace hnurm;
-
-class Settings
-{
-public:
- Settings()
- {
- goodInput = false;
- }
- enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
- enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST };
- void write(FileStorage& fs) const //Write serialization for this class
- {
- fs << "{"
- << "BoardSize_Width" << boardSize.width
- << "BoardSize_Height" << boardSize.height
- << "Square_Size" << squareSize
- << "Calibrate_Pattern" << patternToUse
- << "Calibrate_NrOfFrameToUse" << nrFrames
- << "Calibrate_FixAspectRatio" << aspectRatio
- << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
- << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint
-
- << "Write_DetectedFeaturePoints" << writePoints
- << "Write_extrinsicParameters" << writeExtrinsics
- << "Write_gridPoints" << writeGrid
- << "Write_outputFileName" << outputFileName
-
- << "Show_UndistortedImage" << showUndistorsed
-
- << "Input_FlipAroundHorizontalAxis" << flipVertical
- << "Input_Delay" << delay
- << "Input" << input
- << "}";
- }
- void read(const FileNode& node) //Read serialization for this class
- {
- node["BoardSize_Width" ] >> boardSize.width;
- node["BoardSize_Height"] >> boardSize.height;
- node["Calibrate_Pattern"] >> patternToUse;
- node["Square_Size"] >> squareSize;
- node["Calibrate_NrOfFrameToUse"] >> nrFrames;
- node["Calibrate_FixAspectRatio"] >> aspectRatio;
- node["Write_DetectedFeaturePoints"] >> writePoints;
- node["Write_extrinsicParameters"] >> writeExtrinsics;
- node["Write_gridPoints"] >> writeGrid;
- node["Write_outputFileName"] >> outputFileName;
- node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist;
- node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint;
- node["Calibrate_UseFisheyeModel"] >> useFisheye;
- node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
- node["Show_UndistortedImage"] >> showUndistorsed;
- node["Input"] >> input;
- node["Input_Delay"] >> delay;
- node["Fix_K1"] >> fixK1;
- node["Fix_K2"] >> fixK2;
- node["Fix_K3"] >> fixK3;
- node["Fix_K4"] >> fixK4;
- node["Fix_K5"] >> fixK5;
-
- validate();
- }
- void validate()
- {
- goodInput = true;
- if (boardSize.width <= 0 || boardSize.height <= 0)
- {
- cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << endl;
- goodInput = false;
- }
- if (squareSize <= 10e-6)
- {
- cerr << "Invalid square size " << squareSize << endl;
- goodInput = false;
- }
- if (nrFrames <= 0)
- {
- cerr << "Invalid number of frames " << nrFrames << endl;
- goodInput = false;
- }
-
- if (input.empty()) // Check for valid input
- inputType = INVALID;
- else
- {
- if (input[0] >= '0' && input[0] <= '9')
- {
- stringstream ss(input);
- ss >> cameraID;
- inputType = CAMERA;
- }
- else
- {
- if (isListOfImages(input) && readStringList(input, imageList))
- {
- inputType = IMAGE_LIST;
- nrFrames = (nrFrames < (int) imageList.size()) ? nrFrames : (int) imageList.size();
- }
- else
- inputType = VIDEO_FILE;
- }
- if (inputType == CAMERA)
- {
- inputCapture.OpenCam();
- }
- if (inputType == VIDEO_FILE)
- {
- inputCapture.OpenCam();
- }
- if (inputType != IMAGE_LIST && !true)
- inputType = INVALID;
- }
- if (inputType == INVALID)
- {
- cerr << " Input does not exist: " << input;
- goodInput = false;
- }
-
- flag = 0;
- if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
- if(calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST;
- if(aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO;
- if(fixK1) flag |= CALIB_FIX_K1;
- if(fixK2) flag |= CALIB_FIX_K2;
- if(fixK3) flag |= CALIB_FIX_K3;
- if(fixK4) flag |= CALIB_FIX_K4;
- if(fixK5) flag |= CALIB_FIX_K5;
-
- if (useFisheye) {
- // the fisheye model has its own enum, so overwrite the flags
- flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC;
- if(fixK1) flag |= fisheye::CALIB_FIX_K1;
- if(fixK2) flag |= fisheye::CALIB_FIX_K2;
- if(fixK3) flag |= fisheye::CALIB_FIX_K3;
- if(fixK4) flag |= fisheye::CALIB_FIX_K4;
- if (calibFixPrincipalPoint) flag |= fisheye::CALIB_FIX_PRINCIPAL_POINT;
- }
-
- calibrationPattern = NOT_EXISTING;
- if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD;
- if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID;
- if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
- if (calibrationPattern == NOT_EXISTING)
- {
- cerr << " Camera calibration mode does not exist: " << patternToUse << endl;
- goodInput = false;
- }
- atImageList = 0;
-
- }
- Mat nextImage()
- {
- Mat result;
- if (true)
- {
- ImgInfo ifo;
- inputCapture.SendFrame(ifo);
- Mat view0 = ifo.img;
- view0.copyTo(result);
- }
- else if (atImageList < imageList.size())
- result = imread(imageList[atImageList++], IMREAD_COLOR);
-
- return result;
- }
-
- static bool readStringList( const string& filename, vector& l )
- {
- l.clear();
- FileStorage fs(filename, FileStorage::READ);
- if( !fs.isOpened() )
- return false;
- FileNode n = fs.getFirstTopLevelNode();
- if( n.type() != FileNode::SEQ )
- return false;
- FileNodeIterator it = n.begin(), it_end = n.end();
- for( ; it != it_end; ++it )
- l.push_back((string)*it);
- return true;
- }
-
- static bool isListOfImages( const string& filename)
- {
- string s(filename);
- // Look for file extension
- if( s.find(".xml") == string::npos && s.find(".yaml") == string::npos && s.find(".yml") == string::npos )
- return false;
- else
- return true;
- }
-public:
- Size boardSize; // The size of the board -> Number of items by width and height
- Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric circle pattern
- float squareSize; // The size of a square in your defined unit (point, millimeter,etc).
- int nrFrames; // The number of frames to use from the input for calibration
- float aspectRatio; // The aspect ratio
- int delay; // In case of a video input
- bool writePoints; // Write detected feature points
- bool writeExtrinsics; // Write extrinsic parameters
- bool writeGrid; // Write refined 3D target grid points
- bool calibZeroTangentDist; // Assume zero tangential distortion
- bool calibFixPrincipalPoint; // Fix the principal point at the center
- bool flipVertical; // Flip the captured images around the horizontal axis
- string outputFileName; // The name of the file where to write
- bool showUndistorsed; // Show undistorted images after calibration
- string input; // The input ->
- bool useFisheye; // use fisheye camera model for calibration
- bool fixK1; // fix K1 distortion coefficient
- bool fixK2; // fix K2 distortion coefficient
- bool fixK3; // fix K3 distortion coefficient
- bool fixK4; // fix K4 distortion coefficient
- bool fixK5; // fix K5 distortion coefficient
-
- int cameraID;
- vector imageList;
- size_t atImageList;
- HKcam inputCapture;
- InputType inputType;
- bool goodInput;
- int flag;
-
-private:
- string patternToUse;
-
-
-};
-
-static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings())
-{
- if(node.empty())
- x = default_value;
- else
- x.read(node);
-}
-
-enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
-
-bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
- vector > imagePoints, float grid_width, bool release_object);
-INITIALIZE_EASYLOGGINGPP
-
-int main(int argc, char* argv[])
-{
- const String keys
- = "{help h usage ? | | print this message }"
- "{@settings |default.xml| input setting file }"
- "{d | | actual distance between top-left and top-right corners of "
- "the calibration grid }"
- "{winSize | 11 | Half of search window for cornerSubPix }";
- CommandLineParser parser(argc, argv, keys);
- parser.about("This is a camera calibration sample.\n"
- "Usage: camera_calibration [configuration_file -- default ./default.xml]\n"
- "Near the sample file you'll find the configuration file, which has detailed help of "
- "how to edit it. It may be any OpenCV supported file format XML/YAML.");
- if (!parser.check()) {
- parser.printErrors();
- return 0;
- }
-
- if (parser.has("help")) {
- parser.printMessage();
- return 0;
- }
-
- //! [file_read]
- Settings s;
- const string inputSettingsFile = parser.get(0);
- FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
- if (!fs.isOpened())
- {
- cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
- parser.printMessage();
- return -1;
- }
- fs["Settings"] >> s;
- fs.release(); // close Settings file
- //! [file_read]
-
- //FileStorage fout("settings.yml", FileStorage::WRITE); // write config as YAML
- //fout << "Settings" << s;
-
- if (!s.goodInput)
- {
- cout << "Invalid input detected. Application stopping. " << endl;
- return -1;
- }
-
- int winSize = parser.get("winSize");
-
- float grid_width = s.squareSize * (s.boardSize.width - 1);
- bool release_object = false;
- if (parser.has("d")) {
- grid_width = parser.get("d");
- release_object = true;
- }
-
- vector > imagePoints;
- Mat cameraMatrix, distCoeffs;
- Size imageSize;
- int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
- clock_t prevTimestamp = 0;
- const Scalar RED(0,0,255), GREEN(0,255,0);
- const char ESC_KEY = 27;
-
- //! [get_input]
- for(;;)
- {
- Mat view;
- bool blinkOutput = false;
-
- view = s.nextImage();
-
- //----- If no more image, or got enough, then stop calibration and show result -------------
- if( mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames )
- {
- if (runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width,
- release_object))
- mode = CALIBRATED;
- else
- mode = DETECTION;
- }
- if(view.empty()) // If there are no more images stop the loop
- {
- // if calibration threshold was not reached yet, calibrate now
- if( mode != CALIBRATED && !imagePoints.empty() )
- runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width,
- release_object);
- break;
- }
- //! [get_input]
-
- imageSize = view.size(); // Format input image.
- if( s.flipVertical ) flip( view, view, 0 );
-
- //! [find_pattern]
- vector pointBuf;
-
- bool found;
-
- int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
-
- if(!s.useFisheye) {
- // fast check erroneously fails with high distortions like fisheye
- chessBoardFlags |= CALIB_CB_FAST_CHECK;
- }
-
- switch( s.calibrationPattern ) // Find feature points on the input format
- {
- case Settings::CHESSBOARD:
- found = findChessboardCorners(view, s.boardSize, pointBuf, chessBoardFlags);
- break;
- case Settings::CIRCLES_GRID:
- found = findCirclesGrid(view, s.boardSize, pointBuf);
- break;
- case Settings::ASYMMETRIC_CIRCLES_GRID:
- found = findCirclesGrid(view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID);
- break;
- default:
- found = false;
- break;
- }
- //! [find_pattern]
- //! [pattern_found]
- if ( found) // If done with success,
- {
- // improve the found corners' coordinate accuracy for chessboard
- if (s.calibrationPattern == Settings::CHESSBOARD)
- {
- Mat viewGray;
- cvtColor(view, viewGray, COLOR_BGR2GRAY);
- cornerSubPix(viewGray, pointBuf, Size(winSize, winSize),
- Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.0001));
- }
-
- if (mode == CAPTURING && // For camera only take new samples after delay time
- (!true || clock() - prevTimestamp > s.delay * 1e-3 * CLOCKS_PER_SEC))
- {
- imagePoints.push_back(pointBuf);
- prevTimestamp = clock();
- blinkOutput = true;
- }
-
- // Draw the corners.
- drawChessboardCorners(view, s.boardSize, Mat(pointBuf), found);
- }
- //! [pattern_found]
- //----------------------------- Output Text ------------------------------------------------
- //! [output_text]
- string msg = (mode == CAPTURING) ? "100/100" :
- mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
- int baseLine = 0;
- Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
- Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10);
-
- if( mode == CAPTURING )
- {
- if(s.showUndistorsed)
- msg = format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames );
- else
- msg = format( "%d/%d", (int)imagePoints.size(), s.nrFrames );
- }
-
- putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED);
-
- if( blinkOutput )
- bitwise_not(view, view);
- //! [output_text]
- //------------------------- Video capture output undistorted ------------------------------
- //! [output_undistorted]
- if( mode == CALIBRATED && s.showUndistorsed )
- {
- Mat temp = view.clone();
- if (s.useFisheye)
- cv::fisheye::undistortImage(temp, view, cameraMatrix, distCoeffs);
- else
- undistort(temp, view, cameraMatrix, distCoeffs);
- }
- //! [output_undistorted]
- //------------------------------ Show image and check for input commands -------------------
- //! [await_input]
- namedWindow("Image View", WINDOW_NORMAL);
- imshow("Image View", view);
- char key = (char) waitKey(true ? 50 : s.delay);
-
- if( key == ESC_KEY )
- break;
-
- if( key == 'u' && mode == CALIBRATED )
- s.showUndistorsed = !s.showUndistorsed;
-
- if (true && key == 'g')
- {
- mode = CAPTURING;
- imagePoints.clear();
- }
- //! [await_input]
- }
-
- // -----------------------Show the undistorted image for the image list ------------------------
- //! [show_results]
- if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed )
- {
- Mat view, rview, map1, map2;
-
- if (s.useFisheye)
- {
- Mat newCamMat;
- fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize,
- Matx33d::eye(), newCamMat, 1);
- fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(), newCamMat, imageSize,
- CV_16SC2, map1, map2);
- }
- else
- {
- initUndistortRectifyMap(
- cameraMatrix, distCoeffs, Mat(),
- getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize,
- CV_16SC2, map1, map2);
- }
-
- for(size_t i = 0; i < s.imageList.size(); i++ )
- {
- view = imread(s.imageList[i], IMREAD_COLOR);
- if(view.empty())
- continue;
- remap(view, rview, map1, map2, INTER_LINEAR);
- imshow("Image View", rview);
- char c = (char)waitKey();
- if( c == ESC_KEY || c == 'q' || c == 'Q' )
- break;
- }
- }
- //! [show_results]
-
- return 0;
-}
-
-//! [compute_errors]
-static double computeReprojectionErrors( const vector >& objectPoints,
- const vector >& imagePoints,
- const vector