tf_rosrustにBroadcaster追加してみた

tf_rosrustとは

ROS1のRust Clientであるrosrustを使ったRustのtf2実装です。

arjo129さんのrustros_tfにはじまり、Forkにフォークを重ね、smileroboticsで名前を変えて独自路線で開発を継続しています。

Broadcast実装してみた

tf_rosrustはまだListenerのほうしか実装がなかったので、Broadcasterのほうを実装してみました。(まだ私の個人リポジトリのブランチです（2021/01/07追記：smileroboticsリポジトリにマージされました。）)

これが正しい実装なのかよく分かっていませんがrvizで見た感じ動いてそうでした。

実装内容

tf_broadcaster.rs

use crate::{
    tf_error::TfError,
    transforms::{geometry_msgs::TransformStamped, tf2_msgs::TFMessage},
};

pub struct TfBroadcaster {
    publisher: rosrust::Publisher<TFMessage>,
}

impl TfBroadcaster {
    /// Create a new TfBroadcaster
    pub fn new() -> Self {
        Self {
            publisher: rosrust::publish("/tf", 1000).unwrap(),
        }
    }

    /// Broadcast transform
    pub fn send_transform(&self, tf: TransformStamped) -> Result<(), TfError> {
        let tf_message = TFMessage {
            transforms: vec![tf],
        };
        // TODO: handle error correctly
        self.publisher
            .send(tf_message)
            .map_err(|err| TfError::Rosrust(err.description().to_string()))
    }
}

Example

use tf_rosrust::{TfBroadcaster, TransformStamped};

fn main() {
    rosrust::init("broadcaster");
    let broadcaster = TfBroadcaster::new();

    let rate = rosrust::rate(100.0);
    let mut tf = TransformStamped::default();
    tf.header.frame_id = "base_link".to_string();
    tf.child_frame_id = "camera".to_string();
    tf.transform.rotation.w = 1.0;
    let mut theta = 0.01_f64;
    while rosrust::is_ok() {
        theta += 0.01;
        tf.header.stamp = rosrust::now();
        tf.transform.translation.x = theta.sin();
        tf.transform.translation.y = theta.cos();
        broadcaster.send_transform(tf.clone()).unwrap();
        println!("{:?}", tf);
        rate.sleep();
    }
}

感想

思ってたより簡単に出来ました。ROS１の新しい言語のClientを実用化する上で割と足りてないのがactionとtfなんでtfがあるとよりROS1の言語としてしっかりしますね。

蛇足

この記事は2021年アドベントカレンダーとして書きました。

最初はtf_rosrustをROS2対応をさせようとしたのですが、きれいにROS1と統合するのにめんどくなってBroadcaster実装にしてしまいました。そのうちROS2対応もやってみたいと思ってはいます。