前回は docker 上で ROS 2 環境を作り、TurtleBot シミュレーションを動かしてみました。今回は Rust で ROS 2 ノードを作ってみます！
※ 本記事は、前回の記事に従って事前に docker 環境をセットアップし、TurtleBot シミュレーションが動く環境になっていることを前提としています。
やりたいこと
TurtleBot の footprint を動的に変更することを考えます。具体的には、set_footprint_vertex_number という ROS topic に "data: N" を渡して publish すると、footprint が N 角形（N は 3 以上の整数）になるようにします。下記の例だと五角形になります。
ros2 topic pub /set_footprint_vertex_number std_msgs/msg/UInt8 "{data: 5}※ footprint は障害物回避や経路計画で使われるロボットの大きさ（形状）のことです。
PC 環境
- Thinkpad X1 Intel Core i7
- Ubuntu 24.04
Rust 環境構築
「【ROS2/Rust】rclrs(ros2-rust)でROS2 ノードを作成してみた」の記事と同様に以下を実行することで、docker 環境下で Rust を動かすことができます。
docker exec -it ros2_nav2 bash
sudo apt update
sudo apt install -y cargo git libclang-dev python3-pip python3-vcstool
pip install git+https://github.com/colcon/colcon-cargo.git
pip install git+https://github.com/colcon/colcon-ros-cargo.git
mkdir -p /workspace/src && cd /workspace
git clone https://github.com/ros2-rust/ros2_rust.git src/ros2_rust
vcs import src < src/ros2_rust/ros2_rust_humble.repos
source /opt/ros/humble/setup.sh
colcon buildROS 2 ノード作成
パッケージ作成
docker 内で以下のコマンドを実行し、パッケージを作成しましょう。
cd /workspace/src
cargo new footprint_changer
cd footprint_changerfootrprint_changer 配下の package.xml, Cargo.toml, src/main.rs を以下のように更新してください（package.xml は cargo new で作られないので、ファイルの生成から行いましょう）。
<package format="3">
<name>footprint_changer</name>
<version>0.1.0</version>
<description>Rust-based footprint changer for Nav2</description>
<maintainer email="user@example.com">User</maintainer>
<license>Apache-2.0</license>
<depend>rosidl_default_generators</depend>
<depend>geometry_msgs</depend>
<depend>rclrs</depend>
<export>
<build_type>ament_cargo</build_type>
</export>
</package>[package]
name = "footprint_changer"
version = "0.1.0"
edition = "2021"
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
[dependencies]
geometry_msgs = "*"
rclrs = "*"
rcl_interfaces = "*"
std_msgs = "*"
rand = "0.8"use rclrs::RclrsError;
use std::sync::{Arc, Mutex};
use geometry_msgs::msg::{Polygon, Point32};
use std_msgs::msg::UInt8;
struct FootprintChanger {
node: Arc<rclrs::Node>,
footprint_change_required: Arc<Mutex<bool>>,
vertex_count: Arc<Mutex<u8>>,
_vertex_count_sub: Arc<rclrs::Subscription<UInt8>>,
local_pub: Arc<rclrs::Publisher<Polygon>>,
global_pub: Arc<rclrs::Publisher<Polygon>>,
}
impl FootprintChanger {
fn new(context: &rclrs::Context) -> Result<Self, RclrsError> {
println!("FootprintChanger node started!!");
let node = rclrs::Node::new(context, "footprint_changer")?;
let vertex_count = Arc::new(Mutex::new(3));
let vertex_count_cb = Arc::clone(&vertex_count);
let footprint_change_required = Arc::new(Mutex::new(false));
let footprint_change_required_cb = Arc::clone(&footprint_change_required);
let _vertex_count_sub = node.create_subscription("set_footprint_vertex_number", rclrs::QOS_PROFILE_DEFAULT, move |msg: UInt8| {
*vertex_count_cb.lock().unwrap() = msg.data;
*footprint_change_required_cb.lock().unwrap() = true;
})?;
let local_pub = node.create_publisher("local_costmap/footprint", rclrs::QOS_PROFILE_DEFAULT)?;
let global_pub = node.create_publisher("global_costmap/footprint", rclrs::QOS_PROFILE_DEFAULT)?;
Ok(FootprintChanger {
node,
footprint_change_required,
vertex_count,
_vertex_count_sub,
local_pub,
global_pub,
})
}
fn update_footprint(&self) {
let vertices = *self.vertex_count.lock().unwrap() as usize;
if vertices < 3 {
println!("Error: Vertex count should be greater than 2!!");
return;
}
let footprint = Self::generate_polygon(vertices);
let _ = self.local_pub.publish(footprint.clone());
let _ = self.global_pub.publish(footprint);
}
fn generate_polygon(vertices: usize) -> Polygon {
let mut poly = Polygon { points: vec![] };
let radius = 0.2;
let angle_step = 2.0 * std::f32::consts::PI / vertices as f32;
for i in 0..vertices {
let angle = i as f32 * angle_step;
poly.points.push(Point32 {
x: radius * angle.cos(),
y: radius * angle.sin(),
z: 0.0,
});
}
poly
}
fn set_footprint_change_required_status(&self, status: bool) {
*self.footprint_change_required.lock().unwrap() = status;
}
fn get_footprint_change_required_status(&self) -> bool {
*self.footprint_change_required.lock().unwrap()
}
}
fn main() -> Result<(), RclrsError> {
let context = rclrs::Context::new(std::env::args())?;
let node = Arc::new(FootprintChanger::new(&context)?);
let pub_node = Arc::clone(&node);
std::thread::spawn(move || {
loop {
use std::time::Duration;
std::thread::sleep(Duration::from_millis(100));
if pub_node.get_footprint_change_required_status() == true {
println!("Update footprint!!");
pub_node.update_footprint();
pub_node.set_footprint_change_required_status(false);
}
}
});
let _ = rclrs::spin(Arc::clone(&node.node));
Ok(())
}ビルド & 実行
docker 内で以下のコマンドを実行し、ソースコードのビルドを行ってください。
cd /workspace
source install/setup.bash
colcon build複数の terminal で以下を実行していきます。
terminal 1: TurtleBot シミュレーションの実行
docker exec -it ros2_nav2 bash
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=Falseterminal 2: footprint_changer node の実行
docker exec -it ros2_nav2 bash
source install/setup.bash
ros2 run footprint_changer footprint_changerterminal 3: footprint を五角形にするトピックを呼ぶ。
docker exec -it ros2_nav2 bash
ros2 topic pub /set_footprint_vertex_number std_msgs/msg/UInt8 "{data: 5}以下の動画では、9 秒目くらいでロボットの footprint が丸から五角形に変わっています。
"data: 2" とすると多角形を構成できないので Error が出力されることも確認できます。
Error: Vertex count should be greater than 2!!ソースコード解説
変数を定義しています。Arc (Atomic Reference Counted) というのは thread-safe な参照カウント型のスマートポインタです（参考1, 参考2）。
struct FootprintChanger {
node: Arc<rclrs::Node>,
footprint_change_required: Arc<Mutex<bool>>,
vertex_count: Arc<Mutex<u8>>,
_vertex_count_sub: Arc<rclrs::Subscription<UInt8>>,
local_pub: Arc<rclrs::Publisher<Polygon>>,
global_pub: Arc<rclrs::Publisher<Polygon>>,
}_vertex_count_sub は最初に _ をつけないと以下のようにコンパイラに怒られます。後々参照されない変数の先頭には _ をつけておきましょう。
warning: field `vertex_count_sub` is never read
--> src/main.rs:10:5
|
6 | struct FootprintChanger {
| ---------------- field in this struct
...
10 | vertex_count_sub: Arc<rclrs::Subscription<UInt8>>,
| ^^^^^^^^^^^^^^^^
|
= note: `#[warn(dead_code)]` on by defaultコールバック関数に変数の所有権が移ってしまい、以降のコードで変数を参照できなくなるのを防ぐために、vertex_count_cb と footprint_change_required_cb を Arc::clone で作成し、それらをコールバック関数に渡しています。
let vertex_count = Arc::new(Mutex::new(3));
let vertex_count_cb = Arc::clone(&vertex_count);
let footprint_change_required = Arc::new(Mutex::new(false));
let footprint_change_required_cb = Arc::clone(&footprint_change_required);
let _vertex_count_sub = node.create_subscription("set_footprint_vertex_number", rclrs::QOS_PROFILE_DEFAULT, move |msg: UInt8| {
*vertex_count_cb.lock().unwrap() = msg.data;
*footprint_change_required_cb.lock().unwrap() = true;
})?;local_costmap, global_costmap 両方の footprint を更新するために、以下の 2 つの publisher を定義します。
let local_pub = node.create_publisher("local_costmap/footprint", rclrs::QOS_PROFILE_DEFAULT)?;
let global_pub = node.create_publisher("global_costmap/footprint", rclrs::QOS_PROFILE_DEFAULT)?;頂点が 3 つ未満だと多角形として成立しないのでその場合は error を出力し、footprint の更新は行わないようにします。
if vertices < 3 {
println!("Error: Vertex count should be greater than 2!!");
return;
}let _ = を省略すると以下のように怒られるので、変数 _ に publish の実行結果を格納します。
let _ = self.local_pub.publish(footprint.clone());
let _ = self.global_pub.publish(footprint);warning: unused `Result` that must be used
--> src/main.rs:46:9
|
46 | self.local_pub.publish(footprint.clone());
| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
= note: this `Result` may be an `Err` variant, which should be handled
= note: `#[warn(unused_must_use)]` on by default
help: use `let _ = ...` to ignore the resulting value
|
46 | let _ = self.local_pub.publish(footprint.clone());
| +++++++以下では、半径 0.2 の円に内接する多角形を生成し Polygon 型の変数 poly に生成された多角形の頂点を格納しています。
fn generate_polygon(vertices: usize) -> Polygon {
let mut poly = Polygon { points: vec![] };
let radius = 0.2;
let angle_step = 2.0 * std::f32::consts::PI / vertices as f32;
for i in 0..vertices {
let angle = i as f32 * angle_step;
poly.points.push(Point32 {
x: radius * angle.cos(),
y: radius * angle.sin(),
z: 0.0,
});
}
poly
}footprint_change_required は外部から変更・参照したいので setter・getter 関数を用意しました。
fn set_footprint_change_required_status(&self, status: bool) {
*self.footprint_change_required.lock().unwrap() = status;
}
fn get_footprint_change_required_status(&self) -> bool {
*self.footprint_change_required.lock().unwrap()
}Arc::clone() で pub_node を作り、新たに thread を生成してこの thread から footprint を publish します。100ms ごとに footprint_change_required の状態（set_footprint_vertex_number topic が来たときに true になる）を監視して、true であれば update_footprint() メソッドをコールして footprint を更新します。さらに footprint_change_required のフラグを最後に false にします。
let pub_node = Arc::clone(&node);
std::thread::spawn(move || {
loop {
use std::time::Duration;
std::thread::sleep(Duration::from_millis(100));
if pub_node.get_footprint_change_required_status() == true {
println!("Update footprint!!");
pub_node.update_footprint();
pub_node.set_footprint_change_required_status(false);
}
}
});最後に ROS 2 のスピン処理を開始しています。
let _ = rclrs::spin(Arc::clone(&node.node));まとめ
私自身最近 Rust を勉強し始めた初学者なのですが、こちらの Qiita 記事と ChatGPT のおかげで所要時間一日ほどで Rust の ROS 2 ノードを書くことができました！
コメント