Skip to content

Commit 8fc3b62

Browse files
shuhaowushuhaowu
committed
Add examples from ROScon 2024 slides
1 parent 47c88aa commit 8fc3b62

File tree

5 files changed

+205
-1
lines changed

5 files changed

+205
-1
lines changed

‎cmake/ros2.cmake‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,5 +14,6 @@ ament_target_dependencies(cactus_rt
1414

1515
add_subdirectory(examples/ros2/publisher)
1616
add_subdirectory(examples/ros2/subscriber)
17+
add_subdirectory(examples/ros2/roscon2024)
1718

1819
ament_package()

‎examples/ros2/roscon2024/CMakeLists.txt‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
add_executable(cactus_rt_roscon2024_simple_example
2+
simple_example.cc
3+
)
4+
5+
target_link_libraries(cactus_rt_roscon2024_simple_example
6+
PRIVATE
7+
cactus_rt
8+
)
9+
10+
setup_cactus_rt_target_options(cactus_rt_roscon2024_simple_example)
11+
12+
find_package(geometry_msgs REQUIRED)
13+
14+
add_executable(cactus_rt_roscon2024_ros_example
15+
ros_example.cc
16+
)
17+
18+
target_link_libraries(cactus_rt_roscon2024_ros_example
19+
PRIVATE
20+
cactus_rt
21+
)
22+
23+
setup_cactus_rt_target_options(cactus_rt_roscon2024_ros_example)
24+
25+
ament_target_dependencies(cactus_rt_roscon2024_ros_example
26+
PUBLIC
27+
geometry_msgs
28+
)
29+
30+
install(
31+
TARGETS cactus_rt_roscon2024_ros_example
32+
DESTINATION lib/${PROJECT_NAME}
33+
)

‎examples/ros2/roscon2024/ros_example.cc‎

Lines changed: 135 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
#include <cactus_rt/experimental/random.h>
2+
#include <cactus_rt/ros2.h>
3+
#include <cactus_rt/rt.h>
4+
5+
#include <geometry_msgs/msg/twist.hpp>
6+
#include <random>
7+
8+
using namespace std::literals::chrono_literals;
9+
using cactus_rt::experimental::random::RealNumber;
10+
using cactus_rt::ros2::StampedValue;
11+
12+
struct Velocity2D {
13+
double vx;
14+
double vy;
15+
double w;
16+
};
17+
18+
// For demonstration purposes.
19+
void WasteTime(std::chrono::microseconds duration) {
20+
const auto start = cactus_rt::NowNs();
21+
auto duration_ns = duration.count() * 1000;
22+
while (cactus_rt::NowNs() - start < duration_ns) {
23+
}
24+
}
25+
26+
template <>
27+
struct rclcpp::TypeAdapter<Velocity2D, geometry_msgs::msg::Twist> {
28+
using is_specialized = std::true_type;
29+
using custom_type = Velocity2D;
30+
using ros_message_type = geometry_msgs::msg::Twist;
31+
32+
static void convert_to_ros_message(const custom_type& source, ros_message_type& destination) {
33+
destination.linear.x = source.vx;
34+
destination.linear.y = source.vy;
35+
destination.angular.z = source.w;
36+
}
37+
38+
static void convert_to_custom(const ros_message_type& source, custom_type& destination) {
39+
destination.vx = source.linear.x;
40+
destination.vy = source.linear.y;
41+
destination.w = source.angular.z;
42+
}
43+
};
44+
45+
class RT1000 : public cactus_rt::CyclicThread, public cactus_rt::ros2::Ros2ThreadMixin {
46+
static cactus_rt::CyclicThreadConfig MakeConfig() {
47+
cactus_rt::CyclicThreadConfig config;
48+
config.period_ns = 1'000'000; // 1ms period, 1000 Hz
49+
config.SetFifoScheduler(80); // SCHED_FIFO, rtprio = 80
50+
return config;
51+
}
52+
53+
cactus_rt::experimental::random::Xorshift64Rand rand_engine_{std::random_device{}()};
54+
55+
std::shared_ptr<cactus_rt::ros2::SubscriptionLatest<Velocity2D, geometry_msgs::msg::Twist>> cmd_vel_sub_;
56+
std::shared_ptr<cactus_rt::ros2::Publisher<Velocity2D, geometry_msgs::msg::Twist>> feedback_pub_;
57+
58+
public:
59+
RT1000() : CyclicThread("RT1000", MakeConfig()) {
60+
Logger()->set_log_level(quill::LogLevel::Debug);
61+
}
62+
63+
void InitializeForRos2(cactus_rt::ros2::Ros2Adapter& ros2_adapter) final {
64+
cmd_vel_sub_ = ros2_adapter.CreateSubscriptionForLatestValue<Velocity2D, geometry_msgs::msg::Twist>(
65+
"/cmd_vel", rclcpp::QoS(10)
66+
);
67+
68+
feedback_pub_ = ros2_adapter.CreatePublisher<Velocity2D, geometry_msgs::msg::Twist>(
69+
"/cmd_vel_achieved", rclcpp::QoS(10)
70+
);
71+
}
72+
73+
protected:
74+
LoopControl Loop(int64_t /*elapsed_ns*/) noexcept final {
75+
StampedValue<Velocity2D> msg = cmd_vel_sub_->ReadLatest();
76+
77+
Velocity2D achieved_vel;
78+
{
79+
auto span = Tracer().WithSpan("Drive");
80+
achieved_vel = Drive(msg.value.vx, msg.value.vy, msg.value.w);
81+
}
82+
83+
{
84+
auto span = Tracer().WithSpan("Publish");
85+
feedback_pub_->Publish(achieved_vel);
86+
}
87+
88+
LOG_DEBUG(
89+
Logger(),
90+
"Received id = {}; vx, vy, w = {}, {}, {}; Achieved vx, vy, w = {}, {}, {}",
91+
msg.id,
92+
msg.value.vx,
93+
msg.value.vy,
94+
msg.value.w,
95+
achieved_vel.vx,
96+
achieved_vel.vy,
97+
achieved_vel.w
98+
);
99+
100+
return LoopControl::Continue;
101+
}
102+
103+
private:
104+
Velocity2D Drive(double vx, double vy, double w) {
105+
Velocity2D achieved_vel;
106+
if (vx != 0.0) {
107+
achieved_vel.vx = vx + (RealNumber<double>(rand_engine_) - 0.5);
108+
}
109+
110+
if (vy != 0.0) {
111+
achieved_vel.vy = vy + (RealNumber<double>(rand_engine_) - 0.5);
112+
}
113+
114+
if (w != 0.0) {
115+
achieved_vel.w = w + ((RealNumber<double>(rand_engine_) - 0.5) * 0.1);
116+
}
117+
118+
// Waste between 100 - 200 us, uniform
119+
WasteTime(std::chrono::microseconds(static_cast<int64_t>(RealNumber(rand_engine_) * 100.0F + 100.0F)));
120+
121+
return achieved_vel;
122+
}
123+
};
124+
125+
int main(int argc, const char* argv[]) {
126+
cactus_rt::ros2::App app(argc, argv);
127+
128+
auto thread = app.CreateROS2EnabledThread<RT1000>();
129+
130+
app.StartTraceSession("build/ros.perfetto");
131+
app.Start();
132+
app.Join();
133+
134+
return 0;
135+
}

‎examples/ros2/roscon2024/simple_example.cc‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
#include <cactus_rt/rt.h>
2+
3+
using namespace std::literals::chrono_literals;
4+
5+
class RT1000 : public cactus_rt::CyclicThread {
6+
static cactus_rt::CyclicThreadConfig MakeConfig() {
7+
cactus_rt::CyclicThreadConfig config;
8+
config.period_ns = 1'000'000; // 1ms period, 1000 Hz
9+
config.SetFifoScheduler(80); // SCHED_FIFO, rtprio = 80
10+
return config;
11+
}
12+
13+
public:
14+
RT1000() : CyclicThread("RT1000", MakeConfig()) {
15+
Logger()->set_log_level(quill::LogLevel::Debug);
16+
}
17+
18+
protected:
19+
LoopControl Loop(int64_t elapsed_ns) noexcept final {
20+
LOG_DEBUG(Logger(), "This logs every iteration. elapsed={}ns", elapsed_ns);
21+
LOG_DEBUG_LIMIT(1s, Logger(), "This logs every 1s");
22+
return LoopControl::Continue;
23+
}
24+
};
25+
26+
int main() {
27+
cactus_rt::App app;
28+
29+
auto thread = app.CreateThread<RT1000>();
30+
31+
app.Start();
32+
app.Join();
33+
34+
return 0;
35+
}

‎package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,8 @@
1111

1212
<buildtool_depend>ament_cmake</buildtool_depend>
1313

14-
1514
<build_depend>std_msgs</build_depend> <!-- Needed for the example -->
15+
<build_depend>geometry_msgs</build_depend> <!-- Needed for the example -->
1616
<build_depend>protobuf-dev</build_depend>
1717
<exec_depend>protobuf</exec_depend>
1818
<test_depend>gtest</test_depend>

0 commit comments

Comments
 (0)