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
20pub struct Ros2Navigation {
22 action_client: r2r::ActionClient<NavigateToPose::Action>,
23 _node: Node,
25 current_goal: Arc<Mutex<Option<r2r::ActionClientGoal<NavigateToPose::Action>>>>,
26}
27
28impl Ros2Navigation {
29 #[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(); is_done.store(true, Ordering::Relaxed);
101 });
102 utils::wait(is_done_clone).await;
103 *current_goal.lock().unwrap() = None;
104 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 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#[derive(Debug, Clone, Serialize, Deserialize)]
133#[serde(deny_unknown_fields)]
134pub struct Ros2NavigationConfig {
135 pub action_name: String,
137}