arci_ros/
ros_localization_client.rs

1use arci::*;
2use nalgebra as na;
3use schemars::JsonSchema;
4use serde::{Deserialize, Serialize};
5
6use crate::{msg, rosrust_utils::*};
7
8const AMCL_POSE_TOPIC: &str = "/amcl_pose";
9const NO_MOTION_UPDATE_SERVICE: &str = "request_nomotion_update";
10
11#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
12#[serde(deny_unknown_fields)]
13pub struct RosLocalizationClientConfig {
14    pub request_final_nomotion_update_hack: bool,
15    #[serde(default = "default_nomotion_update_service_name")]
16    pub nomotion_update_service_name: String,
17    #[serde(default = "default_amcl_pose_topic_name")]
18    pub amcl_pose_topic_name: String,
19}
20
21/// Build RosLocalizationClient interactively.
22///
23/// # Examples
24///
25/// ```no_run
26/// let client = arci_ros::RosLocalizationClientBuilder::new().request_final_nomotion_update_hack(true).finalize();
27/// ```
28#[derive(Clone, Debug)]
29pub struct RosLocalizationClientBuilder {
30    amcl_pose_topic_name: String,
31    nomotion_update_service_name: String,
32    request_final_nomotion_update_hack: bool,
33}
34
35impl RosLocalizationClientBuilder {
36    /// Create builder
37    ///
38    /// # Examples
39    ///
40    /// ```
41    /// let builder = arci_ros::RosLocalizationClientBuilder::new();
42    /// ```
43    pub fn new() -> Self {
44        Self {
45            amcl_pose_topic_name: AMCL_POSE_TOPIC.to_string(),
46            nomotion_update_service_name: NO_MOTION_UPDATE_SERVICE.to_string(),
47            request_final_nomotion_update_hack: false,
48        }
49    }
50
51    /// Enable/Disable request_final_nomotion_update_hack
52    ///
53    /// # Examples
54    ///
55    /// ```
56    /// // true means enable (default: false)
57    /// let builder = arci_ros::RosLocalizationClientBuilder::new().request_final_nomotion_update_hack(true);
58    /// ```
59    pub fn request_final_nomotion_update_hack(mut self, val: bool) -> Self {
60        self.request_final_nomotion_update_hack = val;
61        self
62    }
63
64    /// Convert builder into RosLocalizationClient finally.
65    ///
66    /// # Examples
67    ///
68    /// ```no_run
69    /// let client = arci_ros::RosLocalizationClientBuilder::new().finalize();
70    /// ```
71    pub fn finalize(self) -> RosLocalizationClient {
72        RosLocalizationClient::new(
73            self.request_final_nomotion_update_hack,
74            self.nomotion_update_service_name,
75            self.amcl_pose_topic_name,
76        )
77    }
78}
79
80impl Default for RosLocalizationClientBuilder {
81    fn default() -> Self {
82        Self::new()
83    }
84}
85
86pub struct RosLocalizationClient {
87    pose_subscriber: SubscriberHandler<msg::geometry_msgs::PoseWithCovarianceStamped>,
88    nomotion_update_client: Option<rosrust::Client<msg::std_srvs::Empty>>,
89    amcl_pose_topic_name: String,
90}
91
92impl RosLocalizationClient {
93    pub fn new(
94        request_final_nomotion_update_hack: bool,
95        nomotion_update_service_name: String,
96        amcl_pose_topic_name: String,
97    ) -> Self {
98        let pose_subscriber = SubscriberHandler::new(&amcl_pose_topic_name, 1);
99        let nomotion_update_client = if request_final_nomotion_update_hack {
100            rosrust::wait_for_service(
101                &nomotion_update_service_name,
102                Some(std::time::Duration::from_secs(10)),
103            )
104            .unwrap();
105            Some(rosrust::client::<msg::std_srvs::Empty>(&nomotion_update_service_name).unwrap())
106        } else {
107            None
108        };
109        Self {
110            pose_subscriber,
111            nomotion_update_client,
112            amcl_pose_topic_name,
113        }
114    }
115
116    pub fn new_from_config(config: RosLocalizationClientConfig) -> Self {
117        Self::new(
118            config.request_final_nomotion_update_hack,
119            config.nomotion_update_service_name,
120            config.amcl_pose_topic_name,
121        )
122    }
123
124    pub fn request_nomotion_update(&self) {
125        self.nomotion_update_client
126            .as_ref()
127            .unwrap()
128            .req(&msg::std_srvs::EmptyReq {})
129            .unwrap()
130            .unwrap();
131    }
132}
133
134impl Localization for RosLocalizationClient {
135    fn current_pose(&self, _frame_id: &str) -> Result<na::Isometry2<f64>, Error> {
136        self.pose_subscriber.wait_message(100);
137        let pose_with_cov_stamped =
138            self.pose_subscriber
139                .get()?
140                .ok_or_else(|| Error::Connection {
141                    message: format!("Failed to get pose from {}", self.amcl_pose_topic_name),
142                })?;
143        let pose: na::Isometry3<f64> = pose_with_cov_stamped.pose.pose.into();
144
145        Ok(na::Isometry2::new(
146            na::Vector2::new(pose.translation.vector[0], pose.translation.vector[1]),
147            pose.rotation.euler_angles().2,
148        ))
149    }
150}
151
152fn default_nomotion_update_service_name() -> String {
153    NO_MOTION_UPDATE_SERVICE.to_string()
154}
155
156fn default_amcl_pose_topic_name() -> String {
157    AMCL_POSE_TOPIC.to_string()
158}
arci_ros/ros_localization_client.rs

arci_ros/
ros_localization_client.rs
