Skip to content

Commit 548367c

Browse files
creeper5820creeper5820
committed
Update visualization example
1 parent 6fea089 commit 548367c

4 files changed

Lines changed: 60 additions & 66 deletions

File tree

‎src/utility/math/solve_armors.cpp‎

Lines changed: 18 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,21 @@
55
namespace rmcs::util {
66

77
constexpr auto generate_corners(double w, double h) {
8+
using T = Eigen::Vector3d;
9+
using R = Eigen::Quaterniond;
10+
811
return std::array {
9-
Eigen::Vector3d { +0.5 * w, +0.0 * h, 0.0 },
10-
Eigen::Vector3d { +0.0 * w, +0.5 * h, 0.0 },
11-
Eigen::Vector3d { -0.5 * w, -0.0 * h, 0.0 },
12-
Eigen::Vector3d { -0.0 * w, -0.5 * h, 0.0 },
12+
std::pair { T { +0.5 * w, +0.0 * h, 0.0 },
13+
R { Eigen::AngleAxisd(+0.0 * std::numbers::pi, T::UnitZ()) } },
14+
15+
std::pair { T { +0.0 * w, +0.5 * h, 0.0 },
16+
R { Eigen::AngleAxisd(+0.5 * std::numbers::pi, T::UnitZ()) } },
17+
18+
std::pair { T { -0.5 * w, -0.0 * h, 0.0 },
19+
R { Eigen::AngleAxisd(+1.0 * std::numbers::pi, T::UnitZ()) } },
20+
21+
std::pair { T { -0.0 * w, -0.5 * h, 0.0 },
22+
R { Eigen::AngleAxisd(-0.5 * std::numbers::pi, T::UnitZ()) } },
1323
};
1424
}
1525

@@ -21,14 +31,12 @@ auto ArmorsForwardSolution::solve() noexcept -> void {
2131
const auto q = input.q.make<Eigen::Quaterniond>();
2232

2333
const auto corners = generate_corners(w, h);
24-
for (auto&& [status, corner] : std::views::zip(result.armors_status, corners)) {
25-
const auto global_translation = Eigen::Vector3d { q * corner + t };
2634

27-
auto point_to_center = Eigen::Vector3d { t - global_translation };
28-
point_to_center.normalize();
35+
for (auto&& [status, local] : std::views::zip(result.armors_status, corners)) {
36+
const auto& [local_pos, local_rot] = local;
2937

30-
const auto global_orientation =
31-
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), point_to_center);
38+
auto global_translation = Eigen::Vector3d { t + q * local_pos };
39+
auto global_orientation = Eigen::Quaterniond { q * local_rot };
3240

3341
std::get<0>(status) = global_translation;
3442
std::get<1>(status) = global_orientation;

‎src/utility/rclcpp/visualization.cpp‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -80,9 +80,11 @@ namespace visual {
8080
auto AssembledArmors::init(
8181
DeviceId device_id, CampColor camp_color, double w, double h) noexcept -> void {
8282
this->w = w, this->h = h;
83-
for (auto& armor : armors) {
83+
for (auto&& [index, armor] : armors | std::views::enumerate) {
8484
context.share_rclcpp_context(armor.context);
8585
armor.init(device_id, camp_color);
86+
armor.context.details->marker_status.ns += std::to_string(index);
87+
armor.context.details->marker_status.id = static_cast<int>(index);
8688
}
8789
}
8890
auto AssembledArmors::update() noexcept -> void {
@@ -106,7 +108,7 @@ namespace visual {
106108
}
107109
}
108110

109-
struct Visualization::Impl {
111+
struct VisualNode::Impl {
110112

111113
std::shared_ptr<rclcpp::Node> rclcpp;
112114

@@ -120,15 +122,15 @@ struct Visualization::Impl {
120122
}
121123
};
122124

123-
auto Visualization::bind_context(visual::details::Context& context) noexcept -> void {
125+
auto VisualNode::bind_context(visual::details::Context& context) noexcept -> void {
124126
pimpl->bind_context(context);
125127
}
126128

127-
Visualization::Visualization(const std::string& id) noexcept
129+
VisualNode::VisualNode(const std::string& id) noexcept
128130
: pimpl { std::make_unique<Impl>(id) } { }
129131

130-
Visualization::Visualization() noexcept { util::panic("Should not use this constructer!"); }
132+
VisualNode::VisualNode() noexcept { util::panic("Should not use this constructer!"); }
131133

132-
Visualization::~Visualization() noexcept = default;
134+
VisualNode::~VisualNode() noexcept = default;
133135

134136
}

‎src/utility/rclcpp/visualization.hpp‎

Lines changed: 5 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,6 @@
99
#include <algorithm>
1010
#include <ranges>
1111

12-
#define tr(string) [] { return string; }
13-
1412
namespace rmcs::util {
1513

1614
namespace visual {
@@ -88,17 +86,15 @@ namespace visual {
8886
static_assert(details::visual_trait<AssembledArmors>);
8987
}
9088

91-
class Visualization {
92-
RMCS_PIMPL_DEFINITION(Visualization)
89+
class VisualNode {
90+
RMCS_PIMPL_DEFINITION(VisualNode)
9391

9492
public:
95-
explicit Visualization(const std::string& id) noexcept;
96-
97-
auto set_topic_prefix(const std::string&) noexcept -> void;
93+
explicit VisualNode(const std::string& id) noexcept;
9894

9995
template <visual::details::visual_trait T>
100-
auto make_visualized(std::string_view id, std::string_view tf) noexcept -> std::unique_ptr<T> {
101-
auto item = std::make_unique<visual::Armor>();
96+
auto make(const std::string& id, const std::string& tf) noexcept -> std::unique_ptr<T> {
97+
auto item = std::make_unique<T>();
10298

10399
if (!visual::details::check_naming(id)) {
104100
util::panic(std::format(

‎test/visualization.cpp‎

Lines changed: 29 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -1,67 +1,55 @@
11
#include "utility/rclcpp/visualization.hpp"
2+
#include "utility/math/solve_armors.hpp"
23
#include <eigen3/Eigen/Dense>
34
#include <print>
4-
#include <rclcpp/rclcpp.hpp>
5+
#include <rclcpp/utilities.hpp>
56

67
auto main() -> int {
78
using namespace rmcs;
89
using namespace rmcs::util;
910

11+
constexpr auto translation_speed = 3.; // m
12+
constexpr auto orientation_speed = 6.28; // rad
13+
1014
rclcpp::init(0, nullptr);
1115

1216
std::println("> Hello World!!!");
1317
std::println("> Visualization Here using Rclcpp");
1418

15-
auto visualization = Visualization { "example" };
19+
auto visual = VisualNode { "example" };
20+
21+
auto armors = std::array<std::unique_ptr<visual::Armor>, 4> {};
22+
auto index = char { 'a' };
23+
std::ranges::for_each(armors, [&](auto& armor) {
24+
const auto naming { std::string { "sentry_" } + index++ };
25+
armor = visual.make<visual::Armor>(naming, "camera_link");
26+
armor->init(DeviceId::SENTRY, CampColor::BLUE);
27+
});
1628

17-
auto armor_a = visualization.make_visualized<visual::Armor>("sentry_a", "camera_link");
18-
auto armor_b = visualization.make_visualized<visual::Armor>("sentry_b", "camera_link");
19-
auto armor_c = visualization.make_visualized<visual::Armor>("sentry_c", "camera_link");
20-
auto armor_d = visualization.make_visualized<visual::Armor>("sentry_d", "camera_link");
21-
armor_a->init(DeviceId::INFANTRY_3, CampColor::RED);
22-
armor_b->init(DeviceId::INFANTRY_3, CampColor::RED);
23-
armor_c->init(DeviceId::INFANTRY_3, CampColor::RED);
24-
armor_d->init(DeviceId::INFANTRY_3, CampColor::RED);
29+
auto solution = ArmorsForwardSolution {};
2530

2631
auto start = std::chrono::steady_clock::now();
2732

2833
while (rclcpp::ok()) {
29-
auto now = std::chrono::steady_clock::now();
30-
double t = std::chrono::duration<double>(now - start).count();
31-
32-
// 整体绕 z 轴旋转：角速度 1 rad/s
33-
double angle = t * 3;
34-
Eigen::AngleAxisd rot(angle, Eigen::Vector3d::UnitZ());
34+
const auto now = std::chrono::steady_clock::now();
35+
const auto t = std::chrono::duration<double>(now - start).count();
3536

36-
// 四个角点（正方形边长 0.5 m）
37-
double half = 0.25;
38-
std::array<Eigen::Vector3d, 4> corners = { Eigen::Vector3d(half, half, 0.0),
39-
Eigen::Vector3d(-half, half, 0.0), Eigen::Vector3d(-half, -half, 0.0),
40-
Eigen::Vector3d(half, -half, 0.0) };
37+
const auto angle_axisd =
38+
Eigen::AngleAxisd { t * orientation_speed, Eigen::Vector3d::UnitZ() };
39+
const auto quaternion = Eigen::Quaterniond { angle_axisd };
4140

42-
// 更新四个装甲板
43-
auto update_armor = [&](const auto& armor, const Eigen::Vector3d& corner) {
44-
Eigen::Vector3d pos = rot * corner;
45-
armor->set_translation(pos);
46-
47-
// 让装甲板朝向中心 (0,0,z)
48-
Eigen::Vector3d to_center = pos;
49-
to_center.normalize();
50-
51-
// 假设装甲板的局部 x 轴是“正前方”
52-
auto orient = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), to_center);
53-
armor->set_orientation(orient);
41+
solution.input.t = std::sin(translation_speed * t) * Eigen::Vector3d::UnitX();
42+
solution.input.q = quaternion;
43+
solution.solve();
5444

45+
const auto& armors_status = solution.result.armors_status;
46+
for (auto&& [armor, status] : std::views::zip(armors, armors_status)) {
47+
armor->set_translation(std::get<0>(status));
48+
armor->set_orientation(std::get<1>(status));
5549
armor->update();
56-
};
57-
58-
update_armor(armor_a, corners[0]);
59-
update_armor(armor_b, corners[1]);
60-
update_armor(armor_c, corners[2]);
61-
update_armor(armor_d, corners[3]);
50+
}
6251

63-
using namespace std::chrono_literals;
64-
rclcpp::sleep_for(5ms);
52+
rclcpp::sleep_for(std::chrono::milliseconds { 1'000 / 60 });
6553
}
6654

6755
return 0;

0 commit comments

Comments
 (0)