robot_behavior/
realtime.rs

1use std::path::Path;
2
3use crate::{RobotException, RobotResult};
4
5pub trait RealtimeBehavior<C, H> {
6    fn enter_realtime(&mut self, realtime_config: C) -> RobotResult<H>;
7    fn exit_realtime(&mut self) -> RobotResult<()>;
8    fn quality_of_service(&self) -> f64;
9}
10
11pub fn is_hardware_realtime() -> bool {
12    if cfg!(target_os = "linux") {
13        Path::new("/sys/kernel/realtime").exists()
14    } else if cfg!(target_os = "windows") {
15        true
16    } else {
17        println!("Unknown OS, assuming realtime kernel.");
18        true
19    }
20}
21
22/// Sets the current thread to the highest possible scheduler priority.
23///
24/// # Errors
25/// * RealtimeException if realtime priority cannot be set for the current thread.
26///
27/// ## Linux
28/// If the method returns an Error please check your /etc/security/limits.conf file
29/// There should be a line like this:
30/// ```text
31///marco            -       rtprio          99
32/// ```
33///
34/// ##
35pub fn set_realtime_priority() -> RobotResult<()> {
36    #[cfg(target_os = "windows")]
37    {
38        use winapi::um::{
39            processthreadsapi::{
40                GetCurrentProcess, GetCurrentThread, SetPriorityClass, SetThreadPriority,
41            },
42            winbase::{REALTIME_PRIORITY_CLASS, THREAD_PRIORITY_TIME_CRITICAL},
43        };
44
45        unsafe {
46            let process_handle = GetCurrentProcess();
47            if SetPriorityClass(process_handle, REALTIME_PRIORITY_CLASS) == 0 {
48                return Err(RobotException::RealtimeException(
49                    "unable to set realtime priority, try run as an adminstrator!".to_string(),
50                ));
51            }
52
53            // 设置当前线程优先级为最高
54            let thread_handle = GetCurrentThread();
55            if SetThreadPriority(thread_handle, THREAD_PRIORITY_TIME_CRITICAL as i32) == 0 {
56                return Err(RobotException::RealtimeException(
57                    "unable to set max thread priority".to_string(),
58                ));
59            }
60        }
61    }
62    #[cfg(target_os = "linux")]
63    unsafe {
64        let max_priority = libc::sched_get_priority_max(libc::SCHED_FIFO);
65        if max_priority == -1 {
66            return Err(RobotException::RealtimeException(
67                "unable to get maximum possible thread priority".to_string(),
68            ));
69        }
70        let thread_param = libc::sched_param {
71            // In the original libfranka the priority is set to the maximum priority (99 in this
72            // case). However, we will set the priority 1 lower as
73            // https://rt.wiki.kernel.org/index.php/HOWTO:_Build_an_RT-application recommends
74            sched_priority: max_priority - 1,
75        };
76        if libc::pthread_setschedparam(libc::pthread_self(), libc::SCHED_FIFO, &thread_param) != 0 {
77            return Err(RobotException::RealtimeException(
78                "unable to set realtime scheduling".to_string(),
79            ));
80        }
81        // The original libfranka does not use mlock. However, we use it to prevent our memory from
82        // being swapped.
83        if libc::mlockall(libc::MCL_CURRENT | libc::MCL_FUTURE) != 0 {
84            return Err(RobotException::RealtimeException(
85                "unable to lock memory".to_string(),
86            ));
87        }
88    }
89
90    Ok(())
91}
robot_behavior/realtime.rs

robot_behavior/
realtime.rs
