arci_ros2/
cmd_vel_move_base.rs

1use std::{
2    sync::{Arc, Mutex, RwLock},
3    time::Duration,
4};
5
6use arci::*;
7use r2r::{geometry_msgs::msg::Twist, nav_msgs::msg::Odometry};
8use serde::{Deserialize, Serialize};
9
10use crate::{utils, Node};
11
12/// `arci::MoveBase` implementation for ROS2.
13pub struct Ros2CmdVelMoveBase {
14    vel_publisher: Mutex<r2r::Publisher<Twist>>,
15    odom: Arc<RwLock<Option<Odometry>>>,
16    odometry_topic_name: String,
17    // keep not to be dropped
18    _node: Node,
19}
20
21impl Ros2CmdVelMoveBase {
22    /// Creates a new `Ros2CmdVelMoveBase` from geometry_msgs/Twist topic name.
23    #[track_caller]
24    pub fn new(node: Node, cmd_topic_name: &str, odometry_topic_name: &str) -> Self {
25        let vel_publisher = node
26            .r2r()
27            .create_publisher(cmd_topic_name, r2r::QosProfile::default())
28            .unwrap();
29
30        let mut odom_subscriber = node
31            .r2r()
32            .subscribe::<Odometry>(odometry_topic_name, r2r::QosProfile::default())
33            .unwrap();
34        let odom = utils::subscribe_one(&mut odom_subscriber, Duration::from_secs(1));
35        let odom = Arc::new(RwLock::new(odom));
36        utils::subscribe_thread(odom_subscriber, odom.clone(), Some);
37
38        Self {
39            vel_publisher: Mutex::new(vel_publisher),
40            odom,
41            odometry_topic_name: odometry_topic_name.to_string(),
42            _node: node,
43        }
44    }
45}
46
47impl MoveBase for Ros2CmdVelMoveBase {
48    fn send_velocity(&self, velocity: &BaseVelocity) -> Result<(), Error> {
49        let mut twist_msg = Twist::default();
50        twist_msg.linear.x = velocity.x;
51        twist_msg.linear.y = velocity.y;
52        twist_msg.angular.z = velocity.theta;
53        self.vel_publisher
54            .lock()
55            .unwrap()
56            .publish(&twist_msg)
57            .map_err(|e| arci::Error::Connection {
58                message: format!("r2r publish error: {e:?}"),
59            })
60    }
61
62    fn current_velocity(&self) -> Result<BaseVelocity, Error> {
63        let subscribed_odom = self.odom.read().unwrap();
64        let current_velocity = match &*subscribed_odom {
65            Some(msg) => BaseVelocity {
66                x: msg.twist.twist.linear.x,
67                y: msg.twist.twist.linear.y,
68                theta: msg.twist.twist.angular.z,
69            },
70            None => {
71                return Err(Error::Connection {
72                    message: format!("Failed to get odom from {}", self.odometry_topic_name),
73                });
74            }
75        };
76        Ok(current_velocity)
77    }
78}
79
80/// Configuration for `Ros2CmdVelMoveBase`.
81#[derive(Debug, Clone, Serialize, Deserialize)]
82#[serde(deny_unknown_fields)]
83pub struct Ros2CmdVelMoveBaseConfig {
84    /// Topic name for geometry_msgs/Twist.
85    pub cmd_vel_topic: String,
86    /// Topic name for nav_msgs/Odometry.
87    pub odom_topic: String,
88}
arci_ros2/cmd_vel_move_base.rs

arci_ros2/
cmd_vel_move_base.rs
