cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bf

Browse Source
This commit is contained in:
cyy_mac
2026-03-25 12:37:08 +08:00
commit 5e1e355ffa
462 changed files with 92376 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
src/rm_utils/CMakeLists.txt Normal file
View File

@@ -0,0 +1,101 @@
cmake_minimum_required(VERSION 3.5)
project(rm_utils)
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -O3)
endif()
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# find package
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(OpenCV 4 REQUIRED)
find_package(Ceres REQUIRED)
find_package(rcpputils REQUIRED)
find_package(fmt REQUIRED)
# include
include_directories(include)
add_library(fytlogger SHARED
src/logger/logger_impl.cpp
src/logger/writer.cpp
src/logger/logger_pool.cpp
)
target_link_libraries(fytlogger fmt::fmt)
ament_target_dependencies(fytlogger fmt)
# create rm_utils lib
add_library(${PROJECT_NAME} SHARED
src/math/utils.cpp
src/math/pnp_solver.cpp
src/math/trajectory_compensator.cpp
src/math/manual_compensator.cpp
src/math/extended_kalman_filter.cpp
src/url_resolver.cpp
src/heartbeat.cpp
)
set(dependencies
rclcpp
rcpputils
Eigen3
eigen3_cmake_module
geometry_msgs
Ceres
OpenCV
)
ament_target_dependencies(${PROJECT_NAME} ${dependencies})
target_include_directories(${PROJECT_NAME} PUBLIC ${OpenCV_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${CERES_LIBRARIES}
)
# Install include directories
install(DIRECTORY include/
DESTINATION include
)
# Install libraries
install(TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)
install(TARGETS fytlogger
EXPORT fytlogger
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)
# export lib lib
ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_targets(fytlogger HAS_LIBRARY_TARGET)
ament_export_dependencies(${dependencies} fmt)
# test
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
list(APPEND AMENT_LINT_AUTO_EXCLUDE
ament_cmake_copyright
ament_cmake_uncrustify
ament_cmake_cpplint
)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

121
src/rm_utils/README.md Normal file
View File

@@ -0,0 +1,121 @@
# rm_utils
## 1. 介绍
该模块提供通用的工具类以及函数，包括：
* 扩展卡尔曼滤波器 ExtendedKalmanFilter
* PnP解算器 PnPSolver
* 弹道补偿器 TrajectoryCompensator
* FYT日志库
* URL路径解析器 URLResolver
* 心跳发布者 HeartBeatPublisher
## 2. 使用方法
### 2.0 扩展卡尔曼滤波
见armor_solver/src/armor_solver_node.cpp
### 2.1 PnP解算
示例：
```cpp
#include "rm_utils/math/pnp_solver.hpp"
// 1. 创建PnP解算器
// 构造函数参数(cv::Mat, cv::Mat, cv::PnPMethod)：相机内参矩阵、畸变系数、解算方法（默认是IPPE）
auto pnp_solver = PnPSolver(camera_matrix, distortion_coefficients);
// 2. 为PnPSolver设置一个物体坐标系
std::vector<cv::Point3f> object_points = {
cv::Point3f(-1.0f, 1.0f, 0.0f),
cv::Point3f(1.0f, 1.0f, 0.0f),
cv::Point3f(1.0f, -1.0f, 0.0f),
cv::Point3f(-1.0f, -1.0f, 0.0f)
};
pnp_solver.setObjectPoints(object_points, "object_frame");
// 3. PnP解算
cv::Mat rvec, tvec;
pnp_solver.solvePnP(image_points, rvec, tvec, "object_frame");
// 对于打符来说，因为以前的PnP是直接返回一个表示位姿的Eigen::VectorXd，所以这里也提供了一个函数，用于将rvec和tvec转换为VectorXd
// 4. 获取位姿
Eigen::VectorXd pose = pnp_solver.getPose(rvec, tvec);
```
### 2.2 弹道补偿
示例：
```cpp
#include "rm_utils/math/trajectory_compensator.hpp"
auto compensator = CompensatorFactory::createCompensator("ideal");
double pitch = 0;
Eigen::Vector3d p = Eigen::Vector3d(1.0, 0, 0);
double temp_pitch = pitch;
if (trajectory_compensator_->compensate(p, temp_pitch)) {
pitch = temp_pitch;
}
```
### 2.3 Eigen和cv::Mat的相互转换
示例：
```cpp
#include "rm_utils/math/utils.hpp"
cv::Mat mat = cv::Mat::eye(3, 3, CV_32FC1);
Eigen::Matrix3f eigen_mat = utils::cvToEigen(mat);
cv::Mat mat2 = utils::eigenToCv(eigen_mat);
```
### 2.4 FYT日志库
示例：
```cpp
#include "rm_utils/logger/log.hpp"
// 1. 初始化
// 参数：日志名称、日志文件路径、日志级别
FYT_REGISTER_LOGGER("test_logger", "/tmp/test_logger.log", INFO);
// 2. 使用
FYT_INFO("test_logger", "This is a test log");
int a = 1;
FYT_WARN("test_logger", "a = {}", a);
```
### 2.5 URL Resolver
能够用很方便的方式（类camera_info_url）访问文件和或ros2包的install目录
示例:
```cpp
#include "rm_utils/url_resolver.hpp"
namespace fs = std::filesystem;
fs::path model_path =
utils::URLResolver::getResolvedPath("package://armor_detector/model/lenet.onnx");
if (fs::exists(model_path)) {
std::cout<<model_path.string()<<std::endl;
}
fs::path file_path = utils::URLResolver::getResolvedPath("file://home/zcf/123.txt");
```
### 2.6 HearBeatPublisher
定时发布心跳数据
示例：
```cpp
#include "rm_utils/heartbeat.hpp"
class TestNode : public rclcp::Node {
public:
TestNode(const rclcpp::NodeOptions& options) : Node("test", opitions) {
heartbeat_ = HeartBeatPublisher::create(this); // 传入Node*
}
private:
HeartBeatPublisher::SharedPtr heartbeat_;
};
```

42
src/rm_utils/include/rm_utils/assert.hpp Normal file
View File

@@ -0,0 +1,42 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_ASSERT_HPP_
#define RM_UTILS_ASSERT_HPP_
#include <iostream>
#include <sstream>
#define FYT_ASSERT(condition) \
do { \
if (!(condition)) { \
std::ostringstream oss; \
oss << "Assertion failed: (" << #condition << ")"; \
std::cerr << oss.str() << std::endl; \
std::abort(); \
} \
} while (0)
#define FYT_ASSERT_MSG(condition, msg) \
do { \
if (!(condition)) { \
std::ostringstream oss; \
oss << "Assertion failed: (" << #condition << ") " << msg; \
std::cerr << oss.str() << std::endl; \
std::abort(); \
} \
} while (0)
#endif

71
src/rm_utils/include/rm_utils/common.hpp Normal file
View File

@@ -0,0 +1,71 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_COMMON_HPP_
#define RM_UTILS_COMMON_HPP_
#include <string>
namespace fyt {
enum class EnemyColor {
RED = 0,
BLUE = 1,
WHITE = 2,
};
inline std::string enemyColorToString(EnemyColor color) {
switch (color) {
case EnemyColor::RED:
return "RED";
case EnemyColor::BLUE:
return "BLUE";
case EnemyColor::WHITE:
return "WHITE";
default:
return "UNKNOWN";
}
}
enum VisionMode {
AUTO_AIM_RED = 0,
AUTO_AIM_BLUE = 1,
SMALL_RUNE_RED = 2,
SMALL_RUNE_BLUE = 3,
BIG_RUNE_RED = 4,
BIG_RUNE_BLUE = 5,
};
inline std::string visionModeToString(VisionMode mode) {
switch (mode) {
case VisionMode::AUTO_AIM_RED:
return "AUTO_AIM_RED";
case VisionMode::AUTO_AIM_BLUE:
return "AUTO_AIM_BLUE";
case VisionMode::SMALL_RUNE_RED:
return "SMALL_RUNE_RED";
case VisionMode::SMALL_RUNE_BLUE:
return "SMALL_RUNE_BLUE";
case VisionMode::BIG_RUNE_RED:
return "BIG_RUNE_RED";
case VisionMode::BIG_RUNE_BLUE:
return "BIG_RUNE_BLUE";
default:
return "UNKNOWN";
}
}
} // namespace fyt
#endif

42
src/rm_utils/include/rm_utils/heartbeat.hpp Normal file
View File

@@ -0,0 +1,42 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_HEARTBEAT_HPP_
#define RM_UTILS_HEARTBEAT_HPP_
// std
#include <memory>
#include <thread>
// ros2
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int64.hpp>
namespace fyt {
class HeartBeatPublisher {
public:
using SharedPtr = std::shared_ptr<HeartBeatPublisher>;
static SharedPtr create(rclcpp::Node *node);
~HeartBeatPublisher();
private:
explicit HeartBeatPublisher(rclcpp::Node *node);
std_msgs::msg::Int64 message_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr publisher_;
std::thread pub_thread_;
};
} // namespace fyt
#endif // RM_UTILS_HEARTBEAT_HPP_

3
src/rm_utils/include/rm_utils/logger/README.md Normal file
View File

@@ -0,0 +1,3 @@
# FYT Logger
一个基于fmt的简单日志库

43
src/rm_utils/include/rm_utils/logger/exception.hpp Normal file
View File

@@ -0,0 +1,43 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_LOGGER_EXCEPTION_HPP_
#define RM_UTILS_LOGGER_EXCEPTION_HPP_
#include <fmt/format.h>
namespace fyt::logger {
class LoggerNotFoundError : public std::exception {
public:
explicit LoggerNotFoundError(std::string_view name) {
msg = fmt::format("Logger {} Not Found", name);
}
const char *what() const noexcept override { return msg.data(); }
private:
std::string msg;
};
class WriteError : public std::exception {
public:
explicit WriteError(std::string_view name) { msg = fmt::format("Write to {} Error", name); }
const char *what() const noexcept override { return msg.data(); }
private:
std::string msg;
};
} // namespace fyt::logger
#endif // RM_UTILS_LOGGER_EXCEPTION_HPP_

44
src/rm_utils/include/rm_utils/logger/impl/global_mutex.hpp Normal file
View File

@@ -0,0 +1,44 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_LOGGER_GLOBAL_MUTEX_HPP_
#define RM_UTILS_LOGGER_GLOBAL_MUTEX_HPP_
#include <mutex>
#include <unordered_map>
namespace fyt::logger {
static std::mutex g_mutex_;
class GlobalMutex {
public:
inline static std::mutex &getConsoleMutex() {
static std::mutex s_mutex;
return s_mutex;
}
inline static std::mutex &getFileMutex(const std::string &filename) {
static std::unordered_map<std::string, std::mutex> file_mutex_map;
std::lock_guard<std::mutex> lock(g_mutex_);
return file_mutex_map[filename];
}
private:
GlobalMutex() = default;
~GlobalMutex() = default;
};
} // namespace fyt::logger
#endif

106
src/rm_utils/include/rm_utils/logger/impl/logger_impl.hpp Normal file
View File

@@ -0,0 +1,106 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_LOGGER_LOGGER_IMPL_HPP_
#define RM_UTILS_LOGGER_LOGGER_IMPL_HPP_
// std
#include <array>
#include <memory>
#include <stdexcept>
#include <string>
#include <unordered_map>
// fmt
#include <fmt/color.h>
#include <fmt/core.h>
// project
#include "rm_utils/logger/impl/writer.hpp"
#include "rm_utils/logger/types.hpp"
namespace fyt::logger {
namespace internal {
class Logger {
public:
Logger(std::string name, std::string path, LogLevel level, LogOptions ops);
template <typename... Args>
void log(LogLevel level, const std::string &format, Args... args) {
std::string log_info = fmt::format(format, args...);
std::string level_prefix = fmt::format("[{}] ", LogNameTable[static_cast<std::uint8_t>(level)]);
std::string name_prefix = fmt::format("[{}] ", name_);
std::string log_time = fmt::format("[{}] ", getLocalTime());
std::string message =
fmt::format("{} {} {}: {}", level_prefix, name_prefix, log_time, log_info);
if (level >= this->level_) {
std::string colored_message =
fmt::format(LogColorTable[static_cast<std::uint8_t>(level)], message);
writer_->write(colored_message);
std::lock_guard<std::mutex> lock(consle_mutex_);
fmt::print(fg(LogFmtColorTable[static_cast<std::uint8_t>(level)]), "{}\n", message);
}
}
template <typename... Args>
void debug(const std::string &format, Args... args) {
log(LogLevel::DEBUG, format, args...);
}
template <typename... Args>
void info(const std::string &format, Args... args) {
log(LogLevel::INFO, format, args...);
}
template <typename... Args>
void warn(const std::string &format, Args... args) {
log(LogLevel::WARN, format, args...);
}
template <typename... Args>
void error(const std::string &format, Args... args) {
log(LogLevel::ERROR, format, args...);
}
template <typename... Args>
void fatal(const std::string &format, Args... args) {
log(LogLevel::FATAL, format, args...);
}
template <typename... Args>
void print(const std::string &format, Args... args) {
std::lock_guard<std::mutex> lock(consle_mutex_);
fmt::print(format, args...);
}
void setLevel(LogLevel level);
void flush();
private:
std::string getLocalTime();
std::mutex &consle_mutex_;
std::string name_;
LogLevel level_;
std::unique_ptr<Writer> writer_;
};
} // namespace internal
} // namespace fyt::logger
#endif // RM_UTILS_LOGGER_LOGGER_IMPL_HPP_

40
src/rm_utils/include/rm_utils/logger/impl/writer.hpp Normal file
View File

@@ -0,0 +1,40 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_LOGGER_WRITER_HPP_
#define RM_UTILS_LOGGER_WRITER_HPP_
// std
#include <fstream>
#include <mutex>
namespace fyt::logger {
class Writer {
public:
explicit Writer(const std::string &filename);
~Writer();
void write(const std::string &message);
void flush();
private:
std::ofstream file_;
std::mutex &r_mutex_;
};
} // namespace fyt::logger
#endif // RM_UTILS_LOGGER_WRITER_HPP_

43
src/rm_utils/include/rm_utils/logger/log.hpp Normal file
View File

@@ -0,0 +1,43 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_LOGGER_LOGGER_HPP_
#define RM_UTILS_LOGGER_LOGGER_HPP_
#include <rm_utils/logger/logger_pool.hpp>
#include <rm_utils/logger/types.hpp>
#define FYT_REGISTER_LOGGER(name, path, level) \
do { \
fyt::logger::LoggerPool::registerLogger( \
name, path, fyt::logger::LogLevel::level, DATE_DIR | DATE_SUFFIX); \
} while (0)
#define FYT_LOG(name, level, ...) \
do { \
fyt::logger::LoggerPool::getLogger(name).log(level, __VA_ARGS__); \
} while (0)
#define FYT_DEBUG(name, ...) FYT_LOG(name, fyt::logger::LogLevel::DEBUG, __VA_ARGS__)
#define FYT_INFO(name, ...) FYT_LOG(name, fyt::logger::LogLevel::INFO, __VA_ARGS__)
#define FYT_WARN(name, ...) FYT_LOG(name, fyt::logger::LogLevel::WARN, __VA_ARGS__)
#define FYT_ERROR(name, ...) FYT_LOG(name, fyt::logger::LogLevel::ERROR, __VA_ARGS__)
#define FYT_FATAL(name, ...) FYT_LOG(name, fyt::logger::LogLevel::FATAL, __VA_ARGS__)
#endif // RM_UTILS_LOGGER_LOGGER_HPP_

47
src/rm_utils/include/rm_utils/logger/logger_pool.hpp Normal file
View File

@@ -0,0 +1,47 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_LOGGER_POOL_HPP_
#define RM_UTILS_LOGGER_POOL_HPP_
#include <memory>
#include <mutex>
#include <string>
#include <unordered_map>
#include "rm_utils/logger/impl/logger_impl.hpp"
namespace fyt::logger {
class LoggerPool {
public:
static internal::Logger &getLogger(const std::string &name);
static void registerLogger(const std::string &name,
const std::string &path,
LogLevel level,
LogOptions = DEFAULT_OPTIONS);
private:
LoggerPool() = default;
~LoggerPool() = default;
LoggerPool(const LoggerPool &) = delete;
LoggerPool &operator=(const LoggerPool &) = delete;
LoggerPool(LoggerPool &&) = delete;
static std::mutex l_mutex_;
static std::unordered_map<std::string, std::shared_ptr<internal::Logger>> loggers_;
};
} // namespace fyt::logger
#endif // RM_UTILS?LOGGER_POOL_HPP_

56
src/rm_utils/include/rm_utils/logger/types.hpp Normal file
View File

@@ -0,0 +1,56 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_LOGGER_TYPES_HPP_
#define RM_UTILS_LOGGER_TYPES_HPP_
// std
#include <algorithm>
#include <cstdint>
#include <exception>
#include <mutex>
#include <unordered_map>
// fmt
#include <fmt/color.h>
#include <fmt/core.h>
namespace fyt::logger {
enum class LogLevel : std::uint8_t { DEBUG, INFO, WARN, ERROR, FATAL };
constexpr const char *LogNameTable[5] = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"};
// DEBUG = gray, INFO = white, WARN = yellow, ERROR = red, FATAL = blue
constexpr const char *LogColorTable[5] = {"<font color=\"#9B9B9B\">{}</font>",
"<font color=\"#FFFFFF\">{}</font>",
"<font color=\"#FFFF00\">{}</font>",
"<font color=\"#FF0000\">{}</font>",
"<font color=\"#0000FF\">{}</font>"};
constexpr fmt::color LogFmtColorTable[5] = {
fmt::color::gray, fmt::color::white, fmt::color::yellow, fmt::color::red, fmt::color::blue};
using LogOptions = unsigned char;
// default options: without date named directory, without date named suffix, append to file
#define DEFAULT_OPTIONS 0b00000000
#define DATE_DIR 0b00000001
#define DATE_SUFFIX 0b00000010
#define OVER_WRITE 0b00000100
} // namespace fyt::logger
#endif // RM_UTILS_LOGGER_TYPES_HPP_

145
src/rm_utils/include/rm_utils/math/extended_kalman_filter.hpp Normal file
View File

@@ -0,0 +1,145 @@
// Copyright Chen Jun 2023. Licensed under the MIT License.
// Copyright xinyang 2021.
//
// Additional modifications and features by Chengfu Zou, Labor. Licensed under Apache License 2.0.
//
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_KALMAN_FILTER_HPP_
#define RM_UTILS_KALMAN_FILTER_HPP_
// std
#include <functional>
// Eigen
#include <Eigen/Dense>
// ceres
#include <ceres/jet.h>
namespace fyt {
// Extended Kalman Filter with auto differentiation
// N_X: state vector dimension
// N_Z: measurement vector dimension
// PredicFunc: process nonlinear vector function
// MeasureFunc: observation nonlinear vector function
template <int N_X, int N_Z, class PredicFunc, class MeasureFunc>
class ExtendedKalmanFilter {
public:
ExtendedKalmanFilter() = default;
using MatrixXX = Eigen::Matrix<double, N_X, N_X>;
using MatrixZX = Eigen::Matrix<double, N_Z, N_X>;
using MatrixXZ = Eigen::Matrix<double, N_X, N_Z>;
using MatrixZZ = Eigen::Matrix<double, N_Z, N_Z>;
using MatrixX1 = Eigen::Matrix<double, N_X, 1>;
using MatrixZ1 = Eigen::Matrix<double, N_Z, 1>;
using UpdateQFunc = std::function<MatrixXX()>;
using UpdateRFunc = std::function<MatrixZZ(const MatrixZ1 &z)>;
explicit ExtendedKalmanFilter(const PredicFunc &f,
const MeasureFunc &h,
const UpdateQFunc &u_q,
const UpdateRFunc &u_r,
const MatrixXX &P0) noexcept
: f(f), h(h), update_Q(u_q), update_R(u_r), P_post(P0) {
F = MatrixXX::Zero();
H = MatrixZX::Zero();
}
// Set the initial state
void setState(const MatrixX1 &x0) noexcept { x_post = x0; }
void setPredictFunc(const PredicFunc &f) noexcept { this->f = f; }
void setMeasureFunc(const MeasureFunc &h) noexcept { this->h = h; }
// Compute a predicted state
MatrixX1 predict() noexcept {
ceres::Jet<double, N_X> x_e_jet[N_X];
for (int i = 0; i < N_X; ++i) {
x_e_jet[i].a = x_post[i];
x_e_jet[i].v[i] = 1.;
// a 对自己的偏导数为 1.
}
ceres::Jet<double, N_X> x_p_jet[N_X];
f(x_e_jet, x_p_jet);
for (int i = 0; i < N_X; ++i) {
x_pri[i] = x_p_jet[i].a;
F.block(i, 0, 1, N_X) = x_p_jet[i].v.transpose();
}
Q = update_Q();
P_pri = F * P_post * F.transpose() + Q;
x_post = x_pri;
return x_pri;
}
// Update the estimated state based on measurement
MatrixX1 update(const MatrixZ1 &z) noexcept {
ceres::Jet<double, N_X> x_p_jet[N_X];
for (int i = 0; i < N_X; i++) {
x_p_jet[i].a = x_pri[i];
x_p_jet[i].v[i] = 1;
}
ceres::Jet<double, N_X> z_p_jet[N_Z];
h(x_p_jet, z_p_jet);
MatrixZ1 z_pri;
for (int i = 0; i < N_Z; i++) {
z_pri[i] = z_p_jet[i].a;
H.block(i, 0, 1, N_X) = z_p_jet[i].v.transpose();
}
R = update_R(z);
K = P_pri * H.transpose() * (H * P_pri * H.transpose() + R).inverse();
x_post = x_post + K * (z - z_pri);
P_post = (MatrixXX::Identity() - K * H) * P_pri;
return x_post;
}
private:
// Process nonlinear vector function
PredicFunc f;
MatrixXX F;
// Observation nonlinear vector function
MeasureFunc h;
MatrixZX H;
// Process noise covariance matrix
UpdateQFunc update_Q;
MatrixXX Q;
// Measurement noise covariance matrix
UpdateRFunc update_R;
MatrixZZ R;
// Priori error estimate covariance matrix
MatrixXX P_pri;
// Posteriori error estimate covariance matrix
MatrixXX P_post;
// Kalman gain
MatrixXZ K;
// Priori state
MatrixX1 x_pri;
// Posteriori state
MatrixX1 x_post;
};
} // namespace fyt
#endif // RM_UTILS_KALMAN_FILTER_HPP_

94
src/rm_utils/include/rm_utils/math/manual_compensator.hpp Normal file
View File

@@ -0,0 +1,94 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_MANUAL_COMPENSATOR_HPP_
#define RM_UTILS_MANUAL_COMPENSATOR_HPP_
#include <vector>
#include <algorithm>
#include <string>
#include <sstream>
namespace fyt {
constexpr size_t NORMAL_STR_NUM = 6;
class LineRegion{
public:
LineRegion(const double l, const double u):
lowwer_(l), upper_(u) {}
bool checkPoint(const double p) const {
if (p > lowwer_ && p < upper_ ) {
return true;
}
return false;
}
bool checkIntersection(const LineRegion& l) const {
if (checkPoint(l.lowwer_) ||
checkPoint(l.upper_)) {
return true;
}
return false;
}
private:
double lowwer_;
double upper_;
};
class ManualCompensator {
public:
struct HeightMapNode {
HeightMapNode(const LineRegion &region, const double pitch, const double yaw):
height_region(region), pitch_offset(pitch), yaw_offset(yaw) {}
LineRegion height_region;
double pitch_offset;
double yaw_offset;
};
struct DistMapNode {
DistMapNode(const LineRegion &region, const std::vector<HeightMapNode> h_nodes):
dist_region(region), height_map(h_nodes) {}
LineRegion dist_region;
std::vector<HeightMapNode> height_map;
};
ManualCompensator() = default;
std::vector<double> angleHardCorrect(const double dist, const double height);
bool updateMap(const LineRegion& d_region,
const LineRegion& h_region,
const double pitch_offset,
const double yaw_offset);
bool updateMapByStr(const std::string& str);
bool updateMapFlow(const std::vector<std::string> strs) {
for (const auto& str: strs) {
if (!updateMapByStr(str)) {
return false;
}
}
return true;
}
private:
bool parseStr(const std::string& str, std::vector<double>& nums);
std::vector<DistMapNode> angle_offset_map_;
};
} // namespace fyt
#endif

168
src/rm_utils/include/rm_utils/math/particle_filter.hpp Normal file
View File

@@ -0,0 +1,168 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_MATH_PARTICLE_FILTER_HPP_
#define RM_UTILS_MATH_PARTICLE_FILTER_HPP_
#include <Eigen/Dense>
#include <random>
#include <set>
namespace fyt {
template <int N_X, int N_Z, int P_NUM>
// 粒子滤波器，目前实现的版本效率不高，可以考虑使用OpenMP等并行计算库进行优化
class ParticleFilter {
using Particles = Eigen::Matrix<double, N_X, P_NUM>;
using Weights = Eigen::Matrix<double, P_NUM, 1>;
using VectorX = Eigen::Matrix<double, N_X, 1>;
using VectorZ = Eigen::Matrix<double, N_Z, 1>;
using MatrixXX = Eigen::Matrix<double, N_X, N_X>;
using MatrixZZ = Eigen::Matrix<double, N_Z, N_Z>;
using PredictFunc = std::function<VectorX(const VectorX &)>;
using MeasureFunc = std::function<VectorZ(const VectorX &)>;
using UpdateQFunc = std::function<MatrixXX()>;
using UpdateRFunc = std::function<MatrixZZ(const VectorZ &z)>;
public:
explicit ParticleFilter(const PredictFunc &f,
const MeasureFunc &h,
const UpdateQFunc &u_q,
const UpdateRFunc &u_r) noexcept
: f(f), h(h), update_q(u_q), update_r(u_r) {
particles_ = Particles::Zero();
weights_ = Weights::Zero();
}
void initState(const VectorX &x0) noexcept {
auto noise = generateRandomNoise(update_q());
particles_ = x0.replicate(1, P_NUM) + noise;
weights_ = Weights::Ones() / P_NUM;
}
void setDim(size_t dim, double val) {
std::random_device rd;
auto gen = std::default_random_engine(rd());
auto process_cov = update_q();
auto normal_distribution = std::normal_distribution<double>(val, process_cov(dim, dim));
for (int i = 0; i < P_NUM; ++i) {
particles_(dim, i) = normal_distribution(gen);
}
}
VectorX predict() {
Particles pred_particles = Particles::Zero();
// 根据状态转移方程进行预测
for (int i = 0; i < P_NUM; i++) {
pred_particles.col(i) = f(particles_.col(i));
}
particles_ = pred_particles;
return particles_ * weights_;
}
VectorX update(const VectorZ &z) {
Weights w = Weights::Zero();
MatrixZZ measurement_cov = update_r(z);
// 基于高斯分布计算权重
for (int i = 0; i < P_NUM; i++) {
VectorZ z_hat = h(particles_.col(i));
double prob = gaussainLikelyhood(z, z_hat, measurement_cov);
w(i) = prob * weights_(i);
}
weights_ = w / w.sum();
VectorX pred = particles_ * weights_;
// n_eff: 有效粒子数
double n_eff = 1.0 / (weights_.transpose() * weights_).value();
// 有效粒子数小于一半时重采样
if (n_eff < P_NUM * 0.5) {
resample();
}
return pred;
}
private:
void resample() {
Particles new_particles = Particles::Zero();
Weights new_weights = Weights::Zero();
Particles noise = generateRandomNoise(update_q());
std::random_device rd;
auto gen = std::default_random_engine(rd());
// 根据权重进行重采样，每个粒子出现概率等于其权重
auto discrete_distribution =
std::discrete_distribution<int>(weights_.data(), weights_.data() + P_NUM);
std::set<int> idx_set;
for (int i = 0; i < P_NUM; i++) {
int idx = discrete_distribution(gen);
new_particles.col(i) = particles_.col(idx);
// 只对已经采样过的粒子添加噪声
// 保证原来的优秀粒子能存活下来，不被噪声覆盖
if (idx_set.find(idx) != idx_set.end()) {
new_particles.col(i) += noise.col(i);
}
new_weights(i) = 1.0 / P_NUM;
idx_set.insert(idx);
}
particles_ = new_particles;
weights_ = new_weights;
}
template <int dim>
double gaussainLikelyhood(const Eigen::Matrix<double, dim, 1> &x,
const Eigen::Matrix<double, dim, 1> &mean,
const Eigen::Matrix<double, dim, dim> &cov) {
// 在服从均值为mean，协方差为cov的高斯分布下，x出现的概率
auto diff = x - mean;
auto exponent = -0.5 * diff.transpose() * cov.inverse() * diff;
return std::exp(exponent) / std::sqrt(std::pow(2 * M_PI, dim) * cov.determinant());
}
// 生成服从高斯分布的随机噪声，假设各个维度之间相互独立
Particles generateRandomNoise(const MatrixXX &cov) {
std::random_device rd;
auto gen = std::default_random_engine(rd());
auto distributions = std::vector<std::normal_distribution<double>>(N_X);
for (int i = 0; i < N_X; ++i) {
distributions[i] = std::normal_distribution<double>(0, cov(i, i));
}
Particles noise = Particles::Zero();
for (int i = 0; i < N_X; ++i) {
for (int j = 0; j < P_NUM; ++j) {
noise(i, j) = distributions[i](gen);
}
}
return noise;
}
private:
PredictFunc f;
MeasureFunc h;
UpdateQFunc update_q;
UpdateRFunc update_r;
Particles particles_;
Weights weights_;
};
} // namespace fyt
#endif

96
src/rm_utils/include/rm_utils/math/pnp_solver.hpp Normal file
View File

@@ -0,0 +1,96 @@
// Created by Chengfu Zou on 2024.1.19
// Copyright(C) FYT Vision Group. All rights resevred.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_PNP_SOLVER_HPP_
#define RM_UTILS_PNP_SOLVER_HPP_
#include <Eigen/Dense>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <unordered_map>
#include <vector>
namespace fyt {
class PnPSolver {
public:
PnPSolver(const std::array<double, 9> &camera_matrix,
const std::vector<double> &distortion_coefficients,
cv::SolvePnPMethod method = cv::SOLVEPNP_IPPE);
// Set an object coord system
void setObjectPoints(const std::string &coord_frame_name,
const std::vector<cv::Point3f> &object_points) noexcept;
// Get 3d position of the object coord system using PnP algorithm
template <class InputArray>
bool solvePnPGeneric(const InputArray &image_points,
std::vector<cv::Mat> &rvecs,
std::vector<cv::Mat> &tvecs,
const std::string &coord_frame_name) {
rvecs.clear();
tvecs.clear();
if (object_points_map_.find(coord_frame_name) != object_points_map_.end()) {
const auto &object_points = object_points_map_[coord_frame_name];
int solutions = cv::solvePnPGeneric(object_points,
image_points,
camera_matrix_,
distortion_coefficients_,
rvecs,
tvecs,
false,
method_);
return solutions > 0;
} else {
return false;
}
}
// Get 3d position of the object coord system using PnP algorithm
template <class InputArray>
bool solvePnP(const InputArray &image_points,
cv::Mat &rvec,
cv::Mat &tvec,
const std::string &coord_frame_name) {
if (object_points_map_.find(coord_frame_name) != object_points_map_.end()) {
const auto &object_points = object_points_map_[coord_frame_name];
return cv::solvePnP(object_points,
image_points,
camera_matrix_,
distortion_coefficients_,
rvec,
tvec,
false,
method_);
} else {
return false;
}
}
// Calculate the distance between armor center and image center
float calculateDistanceToCenter(const cv::Point2f &image_point) const noexcept;
double calculateReprojectionError(const std::vector<cv::Point2f> &image_points,
const cv::Mat &rvec,
const cv::Mat &tvec,
const std::string &coord_frame_name) const noexcept;
private:
std::unordered_map<std::string, std::vector<cv::Point3f>> object_points_map_;
cv::Mat camera_matrix_;
cv::Mat distortion_coefficients_;
cv::SolvePnPMethod method_;
};
} // namespace fyt
#endif // RM_UTILS_PNP_SOLVER_HPP_

85
src/rm_utils/include/rm_utils/math/trajectory_compensator.hpp Normal file
View File

@@ -0,0 +1,85 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_TRAJECTORY_COMPENSATOR_HPP_
#define RM_UTILS_TRAJECTORY_COMPENSATOR_HPP_
#include <Eigen/Dense>
#include <memory>
#include <tuple>
namespace fyt {
class TrajectoryCompensator {
public:
TrajectoryCompensator() = default;
virtual ~TrajectoryCompensator() = default;
// Compensate the trajectory of the bullet, return the pitch increment
bool compensate(const Eigen::Vector3d &target_position, double &pitch) const noexcept;
virtual double getFlyingTime(const Eigen::Vector3d &target_position) const noexcept = 0;
std::vector<std::pair<double, double>> getTrajectory(double distance,
double angle) const noexcept;
double velocity = 15.0;
int iteration_times = 20;
double gravity = 9.8;
double resistance = 0.01;
protected:
// Calculate the trajectory of the bullet, return the vertical impact point
virtual double calculateTrajectory(const double x, const double angle) const noexcept = 0;
};
// IdealCompensator does not consider the air resistance
class IdealCompensator : public TrajectoryCompensator {
public:
double getFlyingTime(const Eigen::Vector3d &target_position) const noexcept override;
protected:
double calculateTrajectory(const double x, const double angle) const noexcept override;
};
// ResistanceCompensator considers the air resistance
class ResistanceCompensator : public TrajectoryCompensator {
public:
double getFlyingTime(const Eigen::Vector3d &target_position) const noexcept override;
protected:
double calculateTrajectory(const double x, const double angle) const noexcept override;
};
// Factory class for trajectory compensator
class CompensatorFactory {
public:
static std::unique_ptr<TrajectoryCompensator> createCompensator(const std::string &type) {
if (type == "ideal") {
return std::make_unique<IdealCompensator>();
} else if (type == "resistance") {
return std::make_unique<ResistanceCompensator>();
} else {
return nullptr;
}
}
private:
CompensatorFactory() = delete;
~CompensatorFactory() = delete;
};
} // namespace fyt
#endif

97
src/rm_utils/include/rm_utils/math/utils.hpp Normal file
View File

@@ -0,0 +1,97 @@
// Created by Chengfu Zou on 2024.1.19
// Copyright(C) FYT Vision Group. All rights resevred.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RM_UTILS_UTILS_HPP_
#define RM_UTILS_UTILS_HPP_
#include <Eigen/Dense>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
namespace fyt {
// util functions
namespace utils {
// Convert euler angles to rotation matrix
enum class EulerOrder { XYZ, XZY, YXZ, YZX, ZXY, ZYX };
template <typename Vec3Like>
Eigen::Matrix3d eulerToMatrix(const Vec3Like &euler, EulerOrder order = EulerOrder::XYZ) {
auto r = Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX());
auto p = Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY());
auto y = Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ());
switch (order) {
case EulerOrder::XYZ:
return (y * p * r).matrix();
case EulerOrder::XZY:
return (p * y * r).matrix();
case EulerOrder::YXZ:
return (y * r * p).matrix();
case EulerOrder::YZX:
return (r * y * p).matrix();
case EulerOrder::ZXY:
return (p * r * y).matrix();
case EulerOrder::ZYX:
return (r * p * y).matrix();
}
}
inline Eigen::Vector3d matrixToEuler(const Eigen::Matrix3d &R,
EulerOrder order = EulerOrder::XYZ) noexcept {
switch (order) {
case EulerOrder::XYZ:
return R.eulerAngles(0, 1, 2);
case EulerOrder::XZY:
return R.eulerAngles(0, 2, 1);
case EulerOrder::YXZ:
return R.eulerAngles(1, 0, 2);
case EulerOrder::YZX:
return R.eulerAngles(1, 2, 0);
case EulerOrder::ZXY:
return R.eulerAngles(2, 0, 1);
case EulerOrder::ZYX:
return R.eulerAngles(2, 1, 0);
}
}
inline Eigen::Vector3d getRPY(const Eigen::Matrix3d &R) {
double yaw = atan2(R(0, 1), R(0, 0));
double c2 = Eigen::Vector2d(R(2, 2), R(1, 2)).norm();
double pitch = atan2(-R(0, 2), c2);
double s1 = sin(yaw);
double c1 = cos(yaw);
double roll = atan2(s1 * R(2, 0) - c1 * R(2, 1), c1 * R(1, 1) - s1 * R(1, 0));
return -Eigen::Vector3d(roll, pitch, yaw);
}
template <typename _Tp, int _rows, int _cols, int _options, int _maxRows, int _maxCols>
cv::Mat eigenToCv(const Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols> &eigen_mat) {
cv::Mat cv_mat;
cv::eigen2cv(eigen_mat, cv_mat);
return cv_mat;
}
inline Eigen::MatrixXd cvToEigen(const cv::Mat &cv_mat) noexcept {
Eigen::MatrixXd eigen_mat = Eigen::MatrixXd::Zero(cv_mat.rows, cv_mat.cols);
cv::cv2eigen(cv_mat, eigen_mat);
return eigen_mat;
}
} // namespace utils
} // namespace fyt
#endif

47
src/rm_utils/include/rm_utils/url_resolver.hpp Normal file
View File

@@ -0,0 +1,47 @@
// Copyright (C) 2021 RoboMaster-OSS
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Additional modifications and features by Chengfu Zou, 2024.
//
// Copyright (C) FYT Vision Group. All rights reserved.
#ifndef RM_UTILS_URL_RESOLVER_HPP_
#define RM_UTILS_URL_RESOLVER_HPP_
#include <filesystem>
#include <string>
namespace fyt::utils {
class URLResolver {
public:
static std::filesystem::path getResolvedPath(const std::string &url);
private:
static std::string resolveUrl(const std::string &url);
URLResolver() = delete;
enum class UrlType {
EMPTY = 0, // empty string
FILE, // file
PACKAGE, // package
INVALID, // anything >= is invalid
};
static UrlType parseUrl(const std::string &url);
static std::string getPackageFileName(const std::string &url);
};
} // namespace fyt::utils
#endif // RM_UTILS_URL_RESOLVER_HPP_

29
src/rm_utils/package.xml Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rm_utils</name>
<version>0.9.0</version>
<description>
robomaster tool package for projectile motion,it's helpful to develop application like Auto-Aim,etc.
</description>
<maintainer email="fyt@csu.edu.cn">zhenpeng ge</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<depend>fmt</depend>
<depend>eigen</depend>
<depend>opencv</depend>
<depend>ceres</depend>
<depend>rcpputils</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

47
src/rm_utils/src/heartbeat.cpp Normal file
View File

@@ -0,0 +1,47 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/heartbeat.hpp"
namespace fyt {
HeartBeatPublisher::SharedPtr HeartBeatPublisher::create(rclcpp::Node *node) {
return std::shared_ptr<HeartBeatPublisher>(new HeartBeatPublisher(node));
}
HeartBeatPublisher::HeartBeatPublisher(rclcpp::Node *node) {
// Initialize message
message_.data = 0;
// Create publisher
std::string node_name = node->get_name();
std::string topic_name = node_name + "/heartbeat";
publisher_ = node->create_publisher<std_msgs::msg::Int64>(topic_name, 1);
// Start publishing thread
pub_thread_ = std::thread([this]() {
while (rclcpp::ok()) {
message_.data++;
publisher_->publish(message_);
std::this_thread::sleep_for(std::chrono::seconds(1));
}
});
}
HeartBeatPublisher::~HeartBeatPublisher() {
if (pub_thread_.joinable()) {
pub_thread_.join();
}
}
} // namespace fyt

72
src/rm_utils/src/logger/logger_impl.cpp Normal file
View File

@@ -0,0 +1,72 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/logger/impl/logger_impl.hpp"
#include <chrono>
#include <iomanip>
#include <string>
#include <utility>
#include "rm_utils/logger/impl/global_mutex.hpp"
#include "rm_utils/logger/types.hpp"
namespace fyt::logger {
namespace internal {
#define CHECK_OPTION(OPS, OPTION) ((OPS & OPTION) == OPTION)
Logger::Logger(std::string name, std::string filename, LogLevel level, LogOptions ops)
: consle_mutex_(GlobalMutex::getConsoleMutex()), name_(name), level_(level) {
std::string cur_date = getLocalTime().substr(0, 10);
if (filename.empty()) {
filename = "./";
}
if (filename.back() != '/') {
filename.append("/");
}
if (filename.front() == '~') {
std::string home_path = std::getenv("HOME");
filename.replace(0, 1, home_path);
}
if (CHECK_OPTION(ops, DATE_DIR)) {
filename.append(cur_date + "/");
}
if (CHECK_OPTION(ops, DATE_SUFFIX)) {
filename.append(name + "_" + cur_date + ".log.md");
} else {
filename.append(name + ".log.md");
}
writer_ = std::make_unique<Writer>(filename);
}
void Logger::setLevel(LogLevel level) { level_ = level; }
void Logger::flush() { writer_->flush(); }
std::string Logger::getLocalTime() {
std::time_t tt = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
// std::tm tm = *std::gmtime(&tt); //GMT (UTC)
std::tm tm = *std::localtime(&tt); //Locale time-zone, usually UTC by default.
std::stringstream ss;
ss << std::put_time(&tm, "%Y-%m-%d %H:%M:%S");
return ss.str();
}
} // namespace internal
} // namespace fyt::logger

55
src/rm_utils/src/logger/logger_pool.cpp Normal file
View File

@@ -0,0 +1,55 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/logger/logger_pool.hpp"
#include <fmt/core.h>
#include "rm_utils/logger/exception.hpp"
namespace fyt::logger {
internal::Logger &LoggerPool::getLogger(const std::string &name) {
std::lock_guard<std::mutex> lock(l_mutex_);
if (auto iter = loggers_.find(name); iter != loggers_.end()) {
return *iter->second;
}
constexpr const char *kDefaultLogPath = "~/fyt2024-log";
constexpr LogOptions kDefaultOptions = DATE_DIR | DATE_SUFFIX;
auto logger = std::make_shared<internal::Logger>(
name, kDefaultLogPath, LogLevel::INFO, kDefaultOptions);
auto [iter, _] = loggers_.emplace(name, logger);
fmt::print(
stderr,
"[WARN] [logger] Logger '{}' not registered, auto-registered with default config\n",
name);
return *iter->second;
}
void LoggerPool::registerLogger(const std::string &name,
const std::string &path,
LogLevel level,
LogOptions ops) {
std::lock_guard<std::mutex> lock(l_mutex_);
if (auto iter = loggers_.find(name); iter == loggers_.end()) {
loggers_[name] = std::make_shared<internal::Logger>(name, path, level, ops);
}
}
std::mutex LoggerPool::l_mutex_;
std::unordered_map<std::string, std::shared_ptr<internal::Logger>> LoggerPool::loggers_;
} // namespace fyt::logger

50
src/rm_utils/src/logger/writer.cpp Normal file
View File

@@ -0,0 +1,50 @@
// Created by Chengfu Zou
// Copyright (c) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/logger/impl/writer.hpp"
#include <filesystem>
#include <fstream>
#include "rm_utils/logger/impl/global_mutex.hpp"
namespace fyt::logger {
Writer::Writer(const std::string &filename) : r_mutex_(GlobalMutex::getFileMutex(filename)) {
auto parent_path = std::filesystem::path(filename).parent_path();
parent_path = parent_path.empty() ? "." : parent_path;
if (!std::filesystem::exists(parent_path)) {
std::filesystem::create_directories(parent_path);
}
file_.open(filename, std::ios::out | std::ios::app);
}
void Writer::write(const std::string &message) {
std::lock_guard<std::mutex> lock(r_mutex_);
file_ << message << "\n\n";
file_.flush();
}
void Writer::flush() {
std::lock_guard<std::mutex> lock(r_mutex_);
file_.flush();
}
Writer::~Writer() {
std::lock_guard<std::mutex> lock(r_mutex_);
file_.close();
}
} // namespace fyt::logger

20
src/rm_utils/src/math/extended_kalman_filter.cpp Normal file
View File

@@ -0,0 +1,20 @@
// Copyright Chen Jun 2023. Licensed under the MIT License.
//
// Additional modifications and features by Chengfu Zou, Labor. Licensed under Apache License 2.0.
//
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/math/extended_kalman_filter.hpp"

105
src/rm_utils/src/math/manual_compensator.cpp Normal file
View File

@@ -0,0 +1,105 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/math/manual_compensator.hpp"
namespace fyt {
bool ManualCompensator::updateMap(const LineRegion& d_region,
const LineRegion& h_region,
const double pitch_offset,
const double yaw_offset) {
auto target_dist_node =
std::find_if(angle_offset_map_.begin(),
angle_offset_map_.end(),
[&](const DistMapNode& dist_node) {
return dist_node.dist_region.checkIntersection(d_region);
});
if (target_dist_node == angle_offset_map_.end()) {
HeightMapNode height_node(h_region, pitch_offset, yaw_offset);
std::vector<HeightMapNode> h_nodes{height_node};
DistMapNode dist_node(d_region, h_nodes);
angle_offset_map_.emplace_back(dist_node);
} else {
auto target_height_node =
std::find_if(target_dist_node->height_map.begin(),
target_dist_node->height_map.end(),
[&](const HeightMapNode& height_node) {
return height_node.height_region.checkIntersection(h_region);
});
if (target_height_node == target_dist_node->height_map.end()) {
HeightMapNode height_node(h_region, pitch_offset, yaw_offset);
target_dist_node->height_map.emplace_back(height_node);
} else {
return false;
}
}
return true;
}
std::vector<double> ManualCompensator::angleHardCorrect(const double dist,
const double height) {
auto target_dist_node =
std::find_if(angle_offset_map_.begin(),
angle_offset_map_.end(),
[&](const DistMapNode& dist_node) {
return dist_node.dist_region.checkPoint(dist);
});
if (target_dist_node != angle_offset_map_.end()) {
auto target_height_node =
std::find_if(target_dist_node->height_map.begin(),
target_dist_node->height_map.end(),
[&](const HeightMapNode& height_node) {
return height_node.height_region.checkPoint(height);
});
if (target_height_node != target_dist_node->height_map.end()) {
return {target_height_node->pitch_offset, target_height_node->yaw_offset};
}
}
return {0.0, 0.0};
}
bool ManualCompensator::parseStr(const std::string& str,
std::vector<double>& nums) {
std::stringstream ss(str);
double num;
while (!ss.eof()) {
ss >> num;
nums.emplace_back(num);
}
if (nums.size() != NORMAL_STR_NUM) {
return false;
}
return true;
}
bool ManualCompensator::updateMapByStr(const std::string &str) {
std::vector<double> nums;
if (!parseStr(str, nums)) {
return false;
}
LineRegion d_region(nums[0], nums[1]);
LineRegion h_region(nums[2], nums[3]);
if (!updateMap(d_region, h_region, nums[4], nums[5])) {
return false;
}
return true;
}
} // namespace fyt

15
src/rm_utils/src/math/particle_filter.cpp Normal file
View File

@@ -0,0 +1,15 @@
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/math/particle_filter.hpp"

59
src/rm_utils/src/math/pnp_solver.cpp Normal file
View File

@@ -0,0 +1,59 @@
// Created by Chengfu Zou on 2024.1.19
// Copyright(C) FYT Vision Group. All rights resevred.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/math/pnp_solver.hpp"
#include <opencv2/calib3d.hpp>
namespace fyt {
PnPSolver::PnPSolver(const std::array<double, 9> &camera_matrix,
const std::vector<double> &distortion_coefficients,
cv::SolvePnPMethod method)
: camera_matrix_(cv::Mat(3, 3, CV_64F, const_cast<double *>(camera_matrix.data())).clone())
, distortion_coefficients_(
cv::Mat(1, 5, CV_64F, const_cast<double *>(distortion_coefficients.data())).clone())
, method_(method) {}
void PnPSolver::setObjectPoints(const std::string &coord_frame_name,
const std::vector<cv::Point3f> &object_points) noexcept {
object_points_map_[coord_frame_name] = object_points;
}
float PnPSolver::calculateDistanceToCenter(const cv::Point2f &image_point) const noexcept {
float cx = camera_matrix_.at<double>(0, 2);
float cy = camera_matrix_.at<double>(1, 2);
return cv::norm(image_point - cv::Point2f(cx, cy));
}
double PnPSolver::calculateReprojectionError(const std::vector<cv::Point2f> &image_points,
const cv::Mat &rvec,
const cv::Mat &tvec,
const std::string &coord_frame_name) const noexcept {
if (object_points_map_.find(coord_frame_name) != object_points_map_.end()) {
const auto &object_points = object_points_map_.at(coord_frame_name);
std::vector<cv::Point2f> reprojected_points;
cv::projectPoints(
object_points, rvec, tvec, camera_matrix_, distortion_coefficients_, reprojected_points);
double error = 0;
for (size_t i = 0; i < image_points.size(); ++i) {
error += cv::norm(image_points[i] - reprojected_points[i]);
}
return error;
} else {
return 0;
}
}
} // namespace fyt

94
src/rm_utils/src/math/trajectory_compensator.cpp Normal file
View File

@@ -0,0 +1,94 @@
// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/math/trajectory_compensator.hpp"
namespace fyt {
bool TrajectoryCompensator::compensate(const Eigen::Vector3d &target_position,
double &pitch) const noexcept {
double target_height = target_position(2);
// The iterative_height is used to calculate angle in each iteration
double iterative_height = target_height;
double impact_height = 0;
double distance =
std::sqrt(target_position(0) * target_position(0) + target_position(1) * target_position(1));
double angle = std::atan2(target_height, distance);
double dh = 0;
// Iterate to find the right angle, which makes the impact height equal to the
// target height
for (int i = 0; i < iteration_times; ++i) {
angle = std::atan2(iterative_height, distance);
if (std::abs(angle) > M_PI / 2.5) {
break;
}
impact_height = calculateTrajectory(distance, angle);
dh = target_height - impact_height;
if (std::abs(dh) < 0.01) {
break;
}
iterative_height += dh;
}
if (std::abs(dh) > 0.01 || std::abs(angle) > M_PI / 2.5) {
return false;
}
pitch = angle;
return true;
}
std::vector<std::pair<double, double>> TrajectoryCompensator::getTrajectory(
double distance, double angle) const noexcept {
std::vector<std::pair<double, double>> trajectory;
if (distance < 0) {
return trajectory;
}
for (double x = 0; x < distance; x += 0.03) {
trajectory.emplace_back(x, calculateTrajectory(x, angle));
}
return trajectory;
}
double IdealCompensator::calculateTrajectory(const double x, const double angle) const noexcept {
double t = x / (velocity * cos(angle));
double y = velocity * sin(angle) * t - 0.5 * gravity * t * t;
return y;
}
double IdealCompensator::getFlyingTime(const Eigen::Vector3d &target_position) const noexcept {
double distance =
sqrt(target_position(0) * target_position(0) + target_position(1) * target_position(1));
double angle = atan2(target_position(2), distance);
double t = distance / (velocity * cos(angle));
return t;
}
double ResistanceCompensator::calculateTrajectory(const double x,
const double angle) const noexcept {
double r = resistance < 1e-4 ? 1e-4 : resistance;
double t = (exp(r * x) - 1) / (r * velocity * cos(angle));
double y = velocity * sin(angle) * t - 0.5 * gravity * t * t;
return y;
}
double ResistanceCompensator::getFlyingTime(const Eigen::Vector3d &target_position) const noexcept {
double r = resistance < 1e-4 ? 1e-4 : resistance;
double distance =
sqrt(target_position(0) * target_position(0) + target_position(1) * target_position(1));
double angle = atan2(target_position(2), distance);
double t = (exp(r * distance) - 1) / (r * velocity * cos(angle));
return t;
}
} // namespace fyt

17
src/rm_utils/src/math/utils.cpp Normal file
View File

@@ -0,0 +1,17 @@
// Created by Chengfu Zou on 2024.1.19
// Copyright(C) FYT Vision Group. All rights resevred.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rm_utils/math/utils.hpp"

144
src/rm_utils/src/url_resolver.cpp Normal file
View File

@@ -0,0 +1,144 @@
// Copyright (C) 2021 RoboMaster-OSS
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Additional modifications and features by Chengfu Zou, 2024.
//
// Copyright (C) FYT Vision Group. All rights reserved.
#include "rm_utils/url_resolver.hpp"
#include <filesystem>
#include "ament_index_cpp/get_package_share_directory.hpp"
#include "rcpputils/env.hpp"
#include "rcpputils/filesystem_helper.hpp"
namespace fyt::utils {
std::filesystem::path URLResolver::getResolvedPath(const std::string &url) {
const std::string resolved_url = resolveUrl(url);
UrlType url_type = parseUrl(url);
std::string res;
switch (url_type) {
case UrlType::EMPTY: {
break;
}
case UrlType::FILE: {
res = resolved_url.substr(7);
break;
}
case UrlType::PACKAGE: {
res = getPackageFileName(resolved_url);
break;
}
default: {
break;
}
}
return std::filesystem::path(res);
}
std::string URLResolver::resolveUrl(const std::string &url) {
std::string resolved;
size_t rest = 0;
while (true) {
// find the next '$' in the URL string
size_t dollar = url.find('$', rest);
if (dollar >= url.length()) {
// no more variables left in the URL
resolved += url.substr(rest);
break;
}
// copy characters up to the next '$'
resolved += url.substr(rest, dollar - rest);
if (url.substr(dollar + 1, 1) != "{") {
// no '{' follows, so keep the '$'
resolved += "$";
} else if (url.substr(dollar + 1, 10) == "{ROS_HOME}") {
// substitute $ROS_HOME
std::string ros_home;
std::string ros_home_env = rcpputils::get_env_var("ROS_HOME");
std::string home_env = rcpputils::get_env_var("HOME");
if (!ros_home_env.empty()) {
// use environment variable
ros_home = ros_home_env;
} else if (!home_env.empty()) {
// use "$HOME/.ros"
ros_home = home_env + "/.ros";
}
resolved += ros_home;
dollar += 10;
} else {
// not a valid substitution variable
resolved += "$"; // keep the bogus '$'
}
// look for next '$'
rest = dollar + 1;
}
return resolved;
}
URLResolver::UrlType URLResolver::parseUrl(const std::string &url) {
if (url == "") {
return UrlType::EMPTY;
}
// Easy C++14 replacement for boost::iequals from :
// https://stackoverflow.com/a/4119881
auto iequals = [](const std::string &a, const std::string &b) {
return std::equal(a.begin(), a.end(), b.begin(), b.end(), [](char a, char b) {
return tolower(a) == tolower(b);
});
};
if (iequals(url.substr(0, 8), "file:///")) {
return UrlType::FILE;
}
if (iequals(url.substr(0, 10), "package://")) {
// look for a '/' following the package name, make sure it is
// there, the name is not empty, and something follows it
size_t rest = url.find('/', 10);
if (rest < url.length() - 1 && rest > 10) {
return UrlType::PACKAGE;
}
}
return UrlType::INVALID;
}
std::string URLResolver::getPackageFileName(const std::string &url) {
// Scan URL from after "package://" until next '/' and extract
// package name. The parseURL() already checked that it's present.
size_t prefix_len = std::string("package://").length();
size_t rest = url.find('/', prefix_len);
std::string package(url.substr(prefix_len, rest - prefix_len));
// Look up the ROS package path name.
std::string pkg_path = ament_index_cpp::get_package_share_directory(package);
if (pkg_path.empty()) { // package not found?
return pkg_path;
} else {
// Construct file name from package location and remainder of URL.
return pkg_path + url.substr(rest);
}
}
} // namespace fyt::utils