arci_ros2/
navigation.rs

1use std::{
2    sync::{
3        atomic::{AtomicBool, Ordering},
4        Arc, Mutex,
5    },
6    time::Duration,
7};
8
9use anyhow::format_err;
10use arci::*;
11use futures::stream::StreamExt;
12use r2r::{
13    builtin_interfaces::msg::Time, geometry_msgs::msg, nav2_msgs::action::NavigateToPose,
14    std_msgs::msg::Header,
15};
16use serde::{Deserialize, Serialize};
17
18use crate::{utils, Node};
19
20/// `arci::Navigation` implementation for ROS2.
21pub struct Ros2Navigation {
22    action_client: r2r::ActionClient<NavigateToPose::Action>,
23    // keep not to be dropped
24    _node: Node,
25    current_goal: Arc<Mutex<Option<r2r::ActionClientGoal<NavigateToPose::Action>>>>,
26}
27
28impl Ros2Navigation {
29    /// Creates a new `Ros2Navigation` from nav2_msgs/NavigateToPose action name.
30    #[track_caller]
31    pub fn new(node: Node, action_name: &str) -> Self {
32        let action_client = node
33            .r2r()
34            .create_action_client::<NavigateToPose::Action>(action_name)
35            .unwrap();
36        Self {
37            action_client,
38            _node: node,
39            current_goal: Arc::new(Mutex::new(None)),
40        }
41    }
42}
43
44fn to_pose_msg(pose: &Isometry2<f64>) -> msg::Pose {
45    let q = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), pose.rotation.angle());
46    msg::Pose {
47        position: msg::Point {
48            x: pose.translation.x,
49            y: pose.translation.y,
50            z: 0.0,
51        },
52        orientation: msg::Quaternion {
53            x: q.coords.x,
54            y: q.coords.y,
55            z: q.coords.z,
56            w: q.coords.w,
57        },
58    }
59}
60
61impl Navigation for Ros2Navigation {
62    fn send_goal_pose(
63        &self,
64        goal: Isometry2<f64>,
65        frame_id: &str,
66        timeout: Duration,
67    ) -> Result<WaitFuture, Error> {
68        let current_goal = self.current_goal.clone();
69        let action_client = self.action_client.clone();
70        let is_available = r2r::Node::is_available(&self.action_client).unwrap();
71        let (sender, receiver) = tokio::sync::oneshot::channel();
72        let frame_id = frame_id.to_owned();
73        tokio::spawn(async move {
74            let is_done = Arc::new(AtomicBool::new(false));
75            let is_done_clone = is_done.clone();
76            let current_goal_clone = current_goal.clone();
77            tokio::spawn(async move {
78                let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
79                let now = clock.get_now().unwrap();
80                let goal = NavigateToPose::Goal {
81                    pose: msg::PoseStamped {
82                        header: Header {
83                            frame_id,
84                            stamp: Time {
85                                sec: now.as_secs() as i32,
86                                nanosec: now.subsec_nanos(),
87                            },
88                        },
89                        pose: to_pose_msg(&goal),
90                    },
91                    ..Default::default()
92                };
93
94                is_available.await.unwrap();
95                let send_goal_request = action_client.send_goal_request(goal).unwrap();
96                let (goal, result, feedback) = send_goal_request.await.unwrap();
97                *current_goal_clone.lock().unwrap() = Some(goal.clone());
98                tokio::spawn(async move { feedback.for_each(|_| std::future::ready(())).await });
99                result.await.unwrap(); // TODO: handle goal state
100                is_done.store(true, Ordering::Relaxed);
101            });
102            utils::wait(is_done_clone).await;
103            *current_goal.lock().unwrap() = None;
104            // TODO: "canceled" should be an error?
105            let _ = sender.send(());
106        });
107        let timeout_fut = tokio::time::sleep(timeout);
108        let wait = WaitFuture::new(async move {
109            tokio::select! {
110                _ = receiver => Ok(()),
111                _ = timeout_fut => Err(arci::Error::Other(format_err!("timeout {timeout:?}"))),
112            }
113        });
114
115        Ok(wait)
116    }
117
118    fn cancel(&self) -> Result<(), Error> {
119        // TODO: current_goal is None until send_goal_request.await is complete.
120        //       Therefore, if cancel is called during that period, it will not work correctly.
121        if let Some(current_goal) = self.current_goal.lock().unwrap().take() {
122            let fut = current_goal.cancel().map_err(|e| Error::Other(e.into()))?;
123            tokio::spawn(async move {
124                let _ = fut.await;
125            });
126        }
127        Ok(())
128    }
129}
130
131/// Configuration for `Ros2Navigation`.
132#[derive(Debug, Clone, Serialize, Deserialize)]
133#[serde(deny_unknown_fields)]
134pub struct Ros2NavigationConfig {
135    /// Action name for nav2_msgs/NavigateToPose.
136    pub action_name: String,
137}