Skip to content

Commit 5412186

Browse files
creeper5820creeper5820
committed
enhance: improve cmake and visualization
1 parent 860d01b commit 5412186

5 files changed

Lines changed: 136 additions & 58 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 63 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -22,9 +22,8 @@ endif()
2222

2323
## 依赖查找
2424
find_package(yaml-cpp REQUIRED)
25-
find_package(OpenCV 4.5 REQUIRED)
25+
find_package(OpenCV REQUIRED)
2626
find_package(OpenVINO REQUIRED)
27-
# find_package(Ceres REQUIRED)
2827

2928
## ROS2 相关依赖
3029
find_package(type_description_interfaces REQUIRED)
@@ -33,32 +32,21 @@ find_package(rclcpp REQUIRED)
3332
find_package(visualization_msgs REQUIRED)
3433
find_package(geometry_msgs REQUIRED)
3534
find_package(tf2_ros REQUIRED)
36-
find_package(rmcs_msgs REQUIRED)
35+
find_package(pluginlib REQUIRED)
3736

37+
find_package(rmcs_msgs REQUIRED)
3838
find_package(rmcs_executor REQUIRED)
3939
find_package(rmcs_description REQUIRED)
4040
find_package(fast_tf REQUIRED)
4141
find_package(hikcamera REQUIRED)
4242

43-
## 目录包含
44-
include_directories(
45-
${PROJECT_SOURCE_DIR}/src/
46-
47-
${type_description_interfaces_INCLUDE_DIRS}
48-
${rclcpp_INCLUDE_DIRS}
49-
${ament_index_cpp_INCLUDE_DIRS}
50-
${visualization_msgs_INCLUDE_DIRS}
51-
${geometry_msgs_INCLUDE_DIRS}
52-
${tf2_ros_INCLUDE_DIRS}
53-
${rmcs_msgs_INCLUDE_DIRS}
54-
55-
${rmcs_executor_INCLUDE_DIRS}
56-
${rmcs_description_INCLUDE_DIRS}
57-
${fast_tf_INCLUDE_DIRS}
58-
${hikcamera_INCLUDE_DIRS}
59-
)
6043

6144
## 代码资源搜索
45+
file(GLOB_RECURSE AUTO_AIM_UTILITY
46+
CONFIGURE_DEPENDS
47+
${PROJECT_SOURCE_DIR}/src/utility/*.cpp
48+
${PROJECT_SOURCE_DIR}/src/utility/*.cc
49+
)
6250
file(GLOB_RECURSE AUTO_AIM_KERNEL
6351
CONFIGURE_DEPENDS
6452
${PROJECT_SOURCE_DIR}/src/kernel/*.cpp
@@ -68,45 +56,87 @@ file(GLOB_RECURSE AUTO_AIM_MODULE
6856
CONFIGURE_DEPENDS
6957
${PROJECT_SOURCE_DIR}/src/module/*.cpp
7058
${PROJECT_SOURCE_DIR}/src/module/*.cc
71-
${PROJECT_SOURCE_DIR}/src/utility/*.cpp
72-
${PROJECT_SOURCE_DIR}/src/utility/*.cc
59+
)
60+
61+
## 基础设施: utility
62+
add_library(
63+
${PROJECT_NAME}_utility SHARED
64+
${AUTO_AIM_UTILITY}
65+
)
66+
target_include_directories(
67+
${PROJECT_NAME}_utility
68+
PUBLIC
69+
${PROJECT_SOURCE_DIR}/src/
70+
${rmcs_msgs_INCLUDE_DIRS}
71+
PRIVATE
72+
${visualization_msgs_INCLUDE_DIRS}
73+
${geometry_msgs_INCLUDE_DIRS}
74+
${tf2_ros_INCLUDE_DIRS}
75+
)
76+
target_link_libraries(
77+
${PROJECT_NAME}_utility
78+
PUBLIC
79+
yaml-cpp::yaml-cpp
80+
ament_index_cpp::ament_index_cpp
81+
rclcpp::rclcpp
82+
${OpenCV_LIBS}
83+
${rmcs_msgs_LIBRARIES}
84+
PRIVATE
85+
${visualization_msgs_LIBRARIES}
86+
${geometry_msgs_LIBRARIES}
87+
${tf2_ros_LIBRARIES}
7388
)
7489

7590
## 核心构建: kernel and module
7691
add_library(
7792
${PROJECT_NAME}_kernel SHARED
7893
${AUTO_AIM_KERNEL}
7994
)
95+
target_include_directories(
96+
${PROJECT_NAME}_kernel
97+
PUBLIC
98+
${PROJECT_SOURCE_DIR}/src/
99+
PRIVATE
100+
${hikcamera_INCLUDE_DIRS}
101+
)
80102
target_link_libraries(
81103
${PROJECT_NAME}_kernel
82-
${OpenCV_LIBS}
83-
yaml-cpp::yaml-cpp
104+
PUBLIC
105+
${PROJECT_NAME}_utility
84106
)
107+
85108
add_library(
86109
${PROJECT_NAME}_module SHARED
87110
${AUTO_AIM_MODULE}
88111
)
112+
target_include_directories(
113+
${PROJECT_NAME}_module
114+
PUBLIC
115+
${PROJECT_SOURCE_DIR}/src/
116+
PRIVATE
117+
${hikcamera_INCLUDE_DIRS}
118+
)
89119
target_link_libraries(
90120
${PROJECT_NAME}_module
91121
PUBLIC
92-
yaml-cpp::yaml-cpp
93-
ament_index_cpp::ament_index_cpp
122+
${PROJECT_NAME}_utility
94123
openvino::runtime
95-
rclcpp::rclcpp
96-
${OpenCV_LIBS}
97-
${hikcamera_LIBRARIES}
98-
${visualization_msgs_LIBRARIES}
99-
${geometry_msgs_LIBRARIES}
100-
${tf2_ros_LIBRARIES}
101124
PRIVATE
102125
tinympcstatic
126+
${hikcamera_LIBRARIES}
103127
)
104128

105129
## 组件构建: component
106130
add_library(
107131
${PROJECT_NAME}_component SHARED
108132
${PROJECT_SOURCE_DIR}/src/component.cpp
109133
)
134+
target_include_directories(
135+
${PROJECT_NAME}_component
136+
PRIVATE
137+
${rmcs_description_INCLUDE_DIRS}
138+
${rmcs_executor_INCLUDE_DIRS}
139+
)
110140
target_link_libraries(
111141
${PROJECT_NAME}_component
112142
${PROJECT_NAME}_kernel
@@ -120,6 +150,7 @@ install(
120150
${PROJECT_NAME}_component
121151
${PROJECT_NAME}_kernel
122152
${PROJECT_NAME}_module
153+
${PROJECT_NAME}_utility
123154
DESTINATION
124155
lib/
125156
)
@@ -132,7 +163,6 @@ install(
132163
)
133164

134165
## plugins for rmcs_executor
135-
find_package(pluginlib REQUIRED)
136166
pluginlib_export_plugin_description_file(
137167
rmcs_executor plugins.xml
138168
)

‎src/component.cpp‎

Lines changed: 29 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,10 @@
88
namespace rmcs {
99

1010
class AutoAimComponent final : public rmcs_executor::Component {
11+
static inline const auto kNaN = std::numeric_limits<double>::quiet_NaN();
12+
static inline const auto kTNaN = Eigen::Vector3d { kNaN, kNaN, kNaN };
13+
static inline const auto kQNaN = Eigen::Quaterniond { kNaN, kNaN, kNaN, kNaN };
14+
1115
private:
1216
AutoAim auto_aim { };
1317

@@ -25,29 +29,36 @@ class AutoAimComponent final : public rmcs_executor::Component {
2529
OutputInterface<double> pitch_acc;
2630
OutputInterface<bool> feedforward_valid;
2731

32+
std::chrono::steady_clock::time_point last_command_timestamp;
33+
2834
public:
2935
explicit AutoAimComponent() noexcept {
3036
register_input("/auto_aim/camera_transform", camera_transform, false);
3137
register_input("/auto_aim/barrel_direction", barrel_direction, false);
3238
register_input("/referee/id", robot_id, false);
3339

3440
register_output("/auto_aim/should_control", should_control, false);
35-
register_output("/auto_aim/control_direction", target_direction, Eigen::Vector3d::Zero());
36-
register_output("/auto_aim/robot_center", robot_center, Eigen::Vector3d::Zero());
41+
register_output("/auto_aim/control_direction", target_direction, kTNaN);
42+
register_output("/auto_aim/robot_center", robot_center, kTNaN);
3743
register_output("/auto_aim/should_shoot", should_shoot, false);
38-
register_output("/auto_aim/yaw_rate", yaw_rate, 0.0);
39-
register_output("/auto_aim/pitch_rate", pitch_rate, 0.0);
40-
register_output("/auto_aim/yaw_acc", yaw_acc, 0.0);
41-
register_output("/auto_aim/pitch_acc", pitch_acc, 0.0);
44+
register_output("/auto_aim/yaw_rate", yaw_rate, kNaN);
45+
register_output("/auto_aim/pitch_rate", pitch_rate, kNaN);
46+
register_output("/auto_aim/yaw_acc", yaw_acc, kNaN);
47+
register_output("/auto_aim/pitch_acc", pitch_acc, kNaN);
4248
register_output("/auto_aim/feedforward_valid", feedforward_valid, false);
4349
}
4450

4551
auto before_updating() -> void override {
46-
if (!camera_transform.ready())
52+
if (!camera_transform.ready()) {
4753
camera_transform.make_and_bind_directly(Eigen::Isometry3d::Identity());
48-
if (!barrel_direction.ready())
54+
}
55+
if (!barrel_direction.ready()) {
4956
barrel_direction.make_and_bind_directly(Eigen::Vector3d::UnitX());
50-
if (!robot_id.ready()) robot_id.make_and_bind_directly(rmcs_msgs::RobotId::UNKNOWN);
57+
}
58+
if (!robot_id.ready()) {
59+
robot_id.make_and_bind_directly(rmcs_msgs::RobotId::UNKNOWN);
60+
}
61+
last_command_timestamp = std::chrono::steady_clock::now();
5162
}
5263

5364
auto update() -> void override {
@@ -67,6 +78,7 @@ class AutoAimComponent final : public rmcs_executor::Component {
6778
ctx.id = *robot_id;
6879
});
6980

81+
const auto now = std::chrono::steady_clock::now();
7082
if (auto_aim.command_updated()) {
7183
auto_aim.with_command([this](const AutoAimState& cmd) {
7284
using namespace std::chrono_literals;
@@ -95,6 +107,14 @@ class AutoAimComponent final : public rmcs_executor::Component {
95107
std::sin(pitch),
96108
};
97109
});
110+
last_command_timestamp = now;
111+
}
112+
113+
using namespace std::chrono_literals;
114+
if (now - last_command_timestamp > 100ms) {
115+
*should_control = false;
116+
*should_shoot = false;
117+
*target_direction = kTNaN;
98118
}
99119
}
100120
};

‎src/kernel/auto_aim.cpp‎

Lines changed: 18 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ struct AutoAim::Impl {
5252

5353
auto context = SystemContext::kIdentity();
5454
{
55-
std::lock_guard lock(self.context_mutex);
55+
std::lock_guard lock { self.context_mutex };
5656
context = self.current_context;
5757
}
5858
visual.publish(context.camera_transform, "camera_link");
@@ -62,8 +62,10 @@ struct AutoAim::Impl {
6262
}
6363

6464
[[maybe_unused]] auto streamer = std::experimental::scope_exit { [&] {
65-
visual.draw_later(
66-
Text { cap.recording() ? "RECORD ON" : "RECORD OFF", { 640, 20 } });
65+
visual.draw_later( // 录制开关
66+
Text { cap.recording() ? "RECORD ON" : "RECORD OFF", { 10, 700 } });
67+
visual.draw_later( // 自瞄帧率
68+
Text { std::format("FPS: {}", framerate.fps()), { 10, 680 } });
6769
visual.update_image(*image);
6870
} };
6971

@@ -139,24 +141,27 @@ struct AutoAim::Impl {
139141
cmd.feedforward_valid = result->feedforward_valid;
140142
cmd.robot_center = result->center_position;
141143

144+
/// TODO:
145+
/// 统一控制侧和自瞄侧的 Pitch 符号方向定义
142146
visual.update_aiming_direction(cmd.yaw, -cmd.pitch);
143147
visual.update_mpc_plan(cmd.yaw, cmd.pitch, cmd.yaw_rate, cmd.pitch_rate,
144148
cmd.yaw_acc, cmd.pitch_acc);
145149
}
146150
}
151+
visual.draw_later(Text { "ATTACK", { 10, 660 }, cmd.should_shoot ? kRed : kWhite });
152+
visual.draw_later(Text { "CONTROL", { 10, 640 }, cmd.should_control ? kRed : kWhite });
147153

148154
{
149-
std::lock_guard lock(self.command_mutex);
155+
std::lock_guard lock { self.command_mutex };
150156
self.current_command = cmd;
151157
self.unread_command.store(true, std::memory_order::release);
152158
}
153159
}
154-
155-
node.shutdown();
160+
RclcppNode::shutdown();
156161
}
157162

158163
explicit Impl(AutoAim& self) {
159-
std::signal(SIGINT, [](int) { util::set_running(false); });
164+
std::signal(SIGINT, +[](int) { util::set_running(false); });
160165

161166
const auto configs = util::configs();
162167
const auto camera_matrix = configs["camera_matrix"].as<std::array<double, 9>>();
@@ -202,6 +207,12 @@ struct AutoAim::Impl {
202207

203208
worker = std::jthread([this, &self](const std::stop_token& stop) { run(self, stop); });
204209
}
210+
~Impl() {
211+
worker.request_stop();
212+
if (worker.joinable()) {
213+
worker.join();
214+
}
215+
}
205216
};
206217

207218
AutoAim::AutoAim() noexcept

‎src/utility/image/drawable.cpp‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -82,20 +82,21 @@ auto Canvas::draw(const Text& text) -> void {
8282
auto& mat = canvas.details().mat;
8383

8484
const auto font = cv::FONT_HERSHEY_SIMPLEX;
85-
const auto scale = 0.6;
86-
const auto white = cv::Scalar { 255, 255, 255, static_cast<double>(transparency) };
87-
const auto black = cv::Scalar { 0, 0, 0, static_cast<double>(transparency) };
85+
const auto scale = 0.5;
86+
87+
const auto font_color = text.color;
88+
const auto back_color = cv::Scalar { 0, 0, 0 };
8889

8990
auto baseline = 0;
9091
const auto size = cv::getTextSize(text.content, font, scale, line_thickness, &baseline);
9192
const auto org = cv::Point2i {
92-
text.center.x - size.width / 2,
93-
text.center.y + size.height / 2,
93+
text.top_left.x,
94+
text.top_left.y + size.height,
9495
};
9596

96-
cv::putText(mat, text.content, org + cv::Point2i { 1, 1 }, font, scale, black,
97+
cv::putText(mat, text.content, org + cv::Point2i { 1, 1 }, font, scale, back_color,
9798
line_thickness + 2, cv::LINE_AA);
98-
cv::putText(mat, text.content, org, font, scale, white, line_thickness, cv::LINE_AA);
99+
cv::putText(mat, text.content, org, font, scale, font_color, line_thickness, cv::LINE_AA);
99100
}
100101

101102
}

‎src/utility/image/drawable.hpp‎

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,13 +4,29 @@
44

55
namespace rmcs {
66

7+
inline const auto kRed = cv::Scalar { 000, 000, 255, 255 };
8+
inline const auto kGreen = cv::Scalar { 000, 255, 000, 255 };
9+
inline const auto kBlue = cv::Scalar { 255, 000, 000, 255 };
10+
inline const auto kYellow = cv::Scalar { 000, 255, 255, 255 };
11+
inline const auto kMagenta = cv::Scalar { 255, 000, 255, 255 };
12+
inline const auto kCyan = cv::Scalar { 255, 255, 000, 255 };
13+
inline const auto kBlack = cv::Scalar { 000, 000, 000, 255 };
14+
inline const auto kWhite = cv::Scalar { 255, 255, 255, 255 };
15+
inline const auto kGray = cv::Scalar { 192, 192, 192, 255 };
16+
inline const auto kDarkGray = cv::Scalar { 128, 128, 128, 255 };
17+
inline const auto kBrown = cv::Scalar { 000, 000, 128, 255 };
18+
inline const auto kOrange = cv::Scalar { 000, 165, 255, 255 };
19+
inline const auto kPurple = cv::Scalar { 128, 000, 128, 255 };
20+
inline const auto kPink = cv::Scalar { 203, 192, 255, 255 };
21+
722
struct Text {
823
std::string content;
9-
cv::Point2i center;
24+
cv::Point2i top_left;
25+
cv::Scalar color = kWhite;
1026
};
1127
struct Area {
1228
cv::Rect2i rect;
13-
cv::Scalar color { 0, 0, 0, 255 };
29+
cv::Scalar color = kWhite;
1430
};
1531

1632
struct Canvas {

0 commit comments

Comments
 (0)