arci_ros2/
ros2_laser_scan.rs

1use std::{
2    sync::{Arc, RwLock},
3    time::Duration,
4};
5
6use arci::*;
7use r2r::{sensor_msgs::msg::LaserScan, QosProfile};
8use serde::{Deserialize, Serialize};
9
10use crate::{utils, Node};
11
12/// `arci::LaserScan2D` implementation for ROS2.
13pub struct Ros2LaserScan2D {
14    scan: Arc<RwLock<Option<LaserScan>>>,
15    laser_scan_topic_name: String,
16    // keep not to be dropped
17    _node: Node,
18}
19
20impl Ros2LaserScan2D {
21    /// Creates a new `Ros2LaserScan2D` from sensor_msgs/LaserScan topic name.
22    pub fn new(node: Node, laser_scan_topic_name: &str) -> Result<Self, Error> {
23        let mut scan_subscriber = node
24            .r2r()
25            .subscribe::<LaserScan>(laser_scan_topic_name, QosProfile::default())
26            .map_err(anyhow::Error::from)?;
27        let scan = utils::subscribe_one(&mut scan_subscriber, Duration::from_secs(1));
28        let scan = Arc::new(RwLock::new(scan));
29        utils::subscribe_thread(scan_subscriber, scan.clone(), Some);
30
31        Ok(Self {
32            scan,
33            laser_scan_topic_name: laser_scan_topic_name.to_owned(),
34            _node: node,
35        })
36    }
37}
38
39impl LaserScan2D for Ros2LaserScan2D {
40    fn current_scan(&self) -> Result<arci::Scan2D, arci::Error> {
41        let subscribed_scan = self.scan.read().unwrap();
42        let current_scan = match &*subscribed_scan {
43            Some(msg) => Scan2D {
44                angle_min: msg.angle_min as f64,
45                angle_max: msg.angle_max as f64,
46                angle_increment: msg.angle_increment as f64,
47                time_increment: msg.time_increment as f64,
48                scan_time: msg.scan_time as f64,
49                range_min: msg.range_min as f64,
50                range_max: msg.range_max as f64,
51                ranges: msg.ranges.iter().map(|&v| v as f64).collect::<Vec<f64>>(),
52                intensities: msg
53                    .intensities
54                    .iter()
55                    .map(|&v| v as f64)
56                    .collect::<Vec<f64>>(),
57            },
58            None => {
59                return Err(Error::Connection {
60                    message: format!("Failed to get scan from {}", self.laser_scan_topic_name),
61                });
62            }
63        };
64        Ok(current_scan)
65    }
66}
67
68/// Configuration for `Ros2LaserScan2D`.
69#[derive(Debug, Clone, Serialize, Deserialize)]
70#[serde(deny_unknown_fields)]
71pub struct Ros2LaserScan2DConfig {
72    /// Topic name for sensor_msgs/LaserScan.
73    pub topic: String,
74}
arci_ros2/ros2_laser_scan.rs

arci_ros2/
ros2_laser_scan.rs
