arci_ros/
ros_localization_client.rs

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

arci_ros/
ros_localization_client.rs
