arci_ros2/
ros2_localization_client.rs

Help
1use std::{
2    sync::{Arc, RwLock},
3    time::Duration,
4};
5
6use arci::{nalgebra as na, *};
7use r2r::{
8    geometry_msgs::msg::{PoseWithCovariance, PoseWithCovarianceStamped},
9    QosProfile,
10};
11use serde::{Deserialize, Serialize};
12use tracing::info;
13
14use crate::{utils, Node};
15
16/// `arci::Localization` implementation for ROS2.
17pub struct Ros2LocalizationClient {
18    // keep not to be dropped
19    _node: Node,
20    nomotion_update_client: Option<r2r::Client<r2r::std_srvs::srv::Empty::Service>>,
21    pose: Arc<RwLock<Option<PoseWithCovariance>>>,
22    amcl_pose_topic_name: String,
23}
24
25impl Ros2LocalizationClient {
26    /// Creates a new `Ros2LaserScan2D`.
27    pub fn new(
28        node: Node,
29        request_final_nomotion_update_hack: bool,
30        nomotion_update_service_name: &str,
31        amcl_pose_topic_name: &str,
32    ) -> Result<Self, Error> {
33        let mut pose_subscriber = node
34            .r2r()
35            .subscribe::<PoseWithCovarianceStamped>(amcl_pose_topic_name, QosProfile::default())
36            .map_err(anyhow::Error::from)?;
37        let pose = utils::subscribe_one(&mut pose_subscriber, Duration::from_secs(1))
38            .map(|pose| pose.pose);
39        let pose = Arc::new(RwLock::new(pose));
40        utils::subscribe_thread(pose_subscriber, pose.clone(), |pose| Some(pose.pose));
41
42        let nomotion_update_client = if request_final_nomotion_update_hack {
43            Some(
44                node.r2r()
45                    .create_client(nomotion_update_service_name, QosProfile::default())
46                    .map_err(anyhow::Error::from)?,
47            )
48        } else {
49            None
50        };
51
52        Ok(Self {
53            _node: node,
54            nomotion_update_client,
55            pose,
56            amcl_pose_topic_name: amcl_pose_topic_name.to_owned(),
57        })
58    }
59
60    /// Request final nomotion update hack
61    pub async fn request_nomotion_update(&self) {
62        match self.nomotion_update_client.as_ref() {
63            Some(client) => {
64                let is_available = r2r::Node::is_available(client).unwrap();
65                is_available.await.unwrap();
66
67                client
68                    .request(&r2r::std_srvs::srv::Empty::Request {})
69                    .unwrap()
70                    .await
71                    .unwrap();
72            }
73            None => info!("Final nomotion update is not requested!"),
74        }
75    }
76}
77
78impl Localization for Ros2LocalizationClient {
79    fn current_pose(&self, _frame_id: &str) -> Result<Isometry2<f64>, Error> {
80        let subscribed_pose = self.pose.read().unwrap();
81        let current_pose = match &*subscribed_pose {
82            Some(msg) => {
83                let u_q = na::UnitQuaternion::from_quaternion(na::Quaternion::new(
84                    msg.pose.orientation.w,
85                    msg.pose.orientation.x,
86                    msg.pose.orientation.y,
87                    msg.pose.orientation.z,
88                ));
89
90                na::Isometry::from_parts(
91                    na::Translation2::new(msg.pose.position.x, msg.pose.position.y),
92                    na::UnitComplex::from_angle(u_q.angle()),
93                )
94            }
95            None => {
96                return Err(Error::Connection {
97                    message: format!("Failed to get pose from {}", self.amcl_pose_topic_name),
98                });
99            }
100        };
101        Ok(current_pose)
102    }
103}
104
105/// Configuration for `Ros2LocalizationClient`.
106#[derive(Debug, Clone, Serialize, Deserialize)]
107#[serde(deny_unknown_fields)]
108pub struct Ros2LocalizationClientConfig {
109    /// Request final nomotion update hack.
110    pub request_final_nomotion_update_hack: bool,
111    /// Service name for std_srvs/Empty.
112    pub nomotion_update_service_name: String,
113    /// Topic name for geometry_msgs/PoseWithCovarianceStamped.
114    pub amcl_pose_topic_name: String,
115}
arci_ros2/ros2_localization_client.rs

arci_ros2/
ros2_localization_client.rs
