arci_ros/
ros_nav_client.rs

1use std::{sync::Arc, time};
2
3use arci::*;
4use nalgebra as na;
5use schemars::JsonSchema;
6use serde::{Deserialize, Serialize};
7
8use crate::{define_action_client, msg, ActionResultWait};
9define_action_client!(MoveBaseActionClient, msg::move_base_msgs, MoveBase);
10
11impl From<na::Isometry2<f64>> for msg::geometry_msgs::Pose {
12    fn from(goal: na::Isometry2<f64>) -> Self {
13        let iso_pose = na::Isometry3::from_parts(
14            na::Translation3::new(goal.translation.x, goal.translation.y, 0.0),
15            na::UnitQuaternion::from_euler_angles(0.0, 0.0, goal.rotation.angle()),
16        );
17        iso_pose.into()
18    }
19}
20
21const NO_MOTION_UPDATE_SERVICE: &str = "request_nomotion_update";
22const MOVE_BASE_ACTION: &str = "/move_base";
23const CLEAR_COSTMAP_SERVICE: &str = "/move_base/clear_costmaps";
24
25/// Build RosNavClient interactively.
26///
27/// # Examples
28///
29/// ```no_run
30/// let client = arci_ros::RosNavClientBuilder::new().clear_costmap_before_start(true).finalize();
31/// ```
32#[derive(Clone, Debug)]
33pub struct RosNavClientBuilder {
34    move_base_action_base_name: String,
35    clear_costmap_service_name: String,
36    nomotion_update_service_name: String,
37    request_final_nomotion_update_hack: bool,
38    clear_costmap_before_start: bool,
39}
40
41impl RosNavClientBuilder {
42    /// Create builder
43    ///
44    /// # Examples
45    ///
46    /// ```
47    /// let builder = arci_ros::RosNavClientBuilder::new();
48    /// ```
49    pub fn new() -> Self {
50        Self {
51            move_base_action_base_name: MOVE_BASE_ACTION.to_string(),
52            clear_costmap_service_name: CLEAR_COSTMAP_SERVICE.to_string(),
53            nomotion_update_service_name: NO_MOTION_UPDATE_SERVICE.to_string(),
54            request_final_nomotion_update_hack: false,
55            clear_costmap_before_start: false,
56        }
57    }
58
59    /// Enable/Disable request_final_nomotion_update_hack
60    ///
61    /// # Examples
62    ///
63    /// ```
64    /// // true means enable (default: false)
65    /// let builder = arci_ros::RosNavClientBuilder::new().request_final_nomotion_update_hack(true);
66    /// ```
67    pub fn request_final_nomotion_update_hack(mut self, val: bool) -> Self {
68        self.request_final_nomotion_update_hack = val;
69        self
70    }
71
72    /// Enable/Disable clear_costmap_before_start
73    ///
74    /// # Examples
75    ///
76    /// ```
77    /// // true means enable (default: false)
78    /// let builder = arci_ros::RosNavClientBuilder::new().clear_costmap_before_start(true);
79    /// ```
80    pub fn clear_costmap_before_start(mut self, val: bool) -> Self {
81        self.clear_costmap_before_start = val;
82        self
83    }
84
85    /// Convert builder into RosNavClient finally.
86    ///
87    /// # Examples
88    ///
89    /// ```no_run
90    /// let client = arci_ros::RosNavClientBuilder::new().finalize();
91    /// ```
92    pub fn finalize(self) -> RosNavClient {
93        let mut c = RosNavClient::new(
94            self.request_final_nomotion_update_hack,
95            self.move_base_action_base_name,
96            self.nomotion_update_service_name,
97            self.clear_costmap_service_name,
98        );
99        c.clear_costmap_before_start = self.clear_costmap_before_start;
100        c
101    }
102}
103
104impl Default for RosNavClientBuilder {
105    fn default() -> Self {
106        Self::new()
107    }
108}
109
110#[derive(Clone)]
111pub struct RosNavClient {
112    pub clear_costmap_before_start: bool,
113    action_client: Arc<MoveBaseActionClient>,
114    nomotion_update_client: Option<rosrust::Client<msg::std_srvs::Empty>>,
115    nomotion_update_service_name: String,
116    clear_costmap_service_name: String,
117}
118
119impl RosNavClient {
120    pub fn new(
121        request_final_nomotion_update_hack: bool,
122        move_base_action_base_name: String,
123        nomotion_update_service_name: String,
124        clear_costmap_service_name: String,
125    ) -> Self {
126        let action_client = MoveBaseActionClient::new(&move_base_action_base_name, 1);
127
128        let nomotion_update_client = if request_final_nomotion_update_hack {
129            rosrust::wait_for_service(
130                &nomotion_update_service_name,
131                Some(std::time::Duration::from_secs(10)),
132            )
133            .unwrap();
134            Some(rosrust::client::<msg::std_srvs::Empty>(&nomotion_update_service_name).unwrap())
135        } else {
136            None
137        };
138        Self {
139            clear_costmap_before_start: false,
140            action_client: Arc::new(action_client),
141            nomotion_update_client,
142            nomotion_update_service_name,
143            clear_costmap_service_name,
144        }
145    }
146
147    pub fn new_from_config(config: RosNavClientConfig) -> Self {
148        let mut s = Self::new(
149            config.request_final_nomotion_update_hack,
150            config.move_base_action_base_name,
151            config.nomotion_update_service_name,
152            config.clear_costmap_service_name,
153        );
154        s.clear_costmap_before_start = config.clear_costmap_before_start;
155        s
156    }
157
158    pub fn clear_costmap(&self) -> Result<(), Error> {
159        // Wait ten seconds for the service to appear
160        rosrust::wait_for_service(
161            &self.clear_costmap_service_name,
162            Some(time::Duration::from_secs(10)),
163        )
164        .unwrap();
165        // Create a client and call service
166        let client =
167            rosrust::client::<msg::std_srvs::Empty>(&self.clear_costmap_service_name).unwrap();
168        client.req(&msg::std_srvs::EmptyReq {}).unwrap().unwrap();
169        rosrust::ros_info!("requested to clear costmaps");
170        Ok(())
171    }
172
173    fn wait_until_reach(
174        &self,
175        mut action_result_wait: ActionResultWait,
176        timeout: std::time::Duration,
177    ) -> Result<(), crate::Error> {
178        match action_result_wait.wait(timeout) {
179            Ok(_) => {
180                rosrust::ros_info!("Action succeeds");
181                if let Some(client) = self.nomotion_update_client.as_ref() {
182                    client.req(&msg::std_srvs::EmptyReq {}).unwrap().unwrap();
183                    rosrust::ros_info!("Called {}", self.nomotion_update_service_name);
184                }
185                Ok(())
186            }
187            Err(e) => match e {
188                crate::Error::ActionResultPreempted(_) => {
189                    rosrust::ros_warn!("Action is cancelled");
190                    Err(e)
191                }
192                _ => {
193                    rosrust::ros_err!("Action does not succeed {e:?}");
194                    Err(e)
195                }
196            },
197        }
198    }
199}
200
201impl Navigation for RosNavClient {
202    fn send_goal_pose(
203        &self,
204        goal: na::Isometry2<f64>,
205        frame_id: &str,
206        timeout: std::time::Duration,
207    ) -> Result<WaitFuture, Error> {
208        if self.clear_costmap_before_start {
209            self.clear_costmap()?;
210        }
211        let mut target_pose = msg::geometry_msgs::PoseStamped {
212            pose: goal.into(),
213            ..Default::default()
214        };
215        frame_id.clone_into(&mut target_pose.header.frame_id);
216        target_pose.header.stamp = rosrust::now();
217        let action_result_wait = self
218            .action_client
219            .send_goal(msg::move_base_msgs::MoveBaseGoal { target_pose })
220            .map_err(|e| anyhow::anyhow!("Failed to send_goal_and_wait : {e}"))?;
221
222        let self_clone = self.clone();
223        // Creates a WaitFuture that waits until reach only if the future
224        // is polled. This future is a bit tricky, but it's more efficient than
225        // using only `tokio::task::spawn_blocking` because it doesn't spawn a thread
226        // if the WaitFuture is ignored.
227        let wait = WaitFuture::new(async move {
228            tokio::task::spawn_blocking(move || {
229                self_clone.wait_until_reach(action_result_wait, timeout)
230            })
231            .await
232            .map_err(|e| arci::Error::Other(e.into()))??;
233            Ok(())
234        });
235
236        Ok(wait)
237    }
238
239    fn cancel(&self) -> Result<(), Error> {
240        self.action_client
241            .cancel_all_goal()
242            .map_err(|e| anyhow::anyhow!("Failed to cancel_all_goal : {e}"))?;
243        Ok(())
244    }
245}
246
247#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
248#[serde(deny_unknown_fields)]
249pub struct RosNavClientConfig {
250    pub request_final_nomotion_update_hack: bool,
251    pub clear_costmap_before_start: bool,
252    #[serde(default = "default_move_base_action_base_name")]
253    pub move_base_action_base_name: String,
254    #[serde(default = "default_nomotion_update_service_name")]
255    pub nomotion_update_service_name: String,
256    #[serde(default = "default_clear_costmap_service_name")]
257    pub clear_costmap_service_name: String,
258}
259
260fn default_move_base_action_base_name() -> String {
261    MOVE_BASE_ACTION.to_string()
262}
263
264fn default_nomotion_update_service_name() -> String {
265    NO_MOTION_UPDATE_SERVICE.to_string()
266}
267
268fn default_clear_costmap_service_name() -> String {
269    CLEAR_COSTMAP_SERVICE.to_string()
270}
arci_ros/ros_nav_client.rs

arci_ros/
ros_nav_client.rs
