-
Notifications
You must be signed in to change notification settings - Fork 4
/
ros2_control.patch
32 lines (28 loc) · 1.37 KB
/
ros2_control.patch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
diff --git a/controller_manager/src/realtime.cpp b/controller_manager/src/realtime.cpp
index 7c77044..0fa5cbc 100644
--- a/controller_manager/src/realtime.cpp
+++ b/controller_manager/src/realtime.cpp
@@ -34,10 +34,7 @@ bool has_realtime_kernel()
bool configure_sched_fifo(int priority)
{
- struct sched_param schedp;
- memset(&schedp, 0, sizeof(schedp));
- schedp.sched_priority = priority;
- return !sched_setscheduler(0, SCHED_FIFO, &schedp);
+ return false;
}
} // namespace controller_manager
diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index b846831..ff91342 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -62,8 +62,9 @@ int main(int argc, char ** argv)
// for calculating sleep time
auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
- std::chrono::system_clock::time_point next_iteration_time =
- std::chrono::system_clock::time_point(std::chrono::nanoseconds(cm->now().nanoseconds()));
+ auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds());
+ std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds>
+ next_iteration_time{cm_now};
// for calculating the measured period of the loop
rclcpp::Time previous_time = cm->now();