mirror
/
r2r
mirror of https://github.com/sequenceplanner/r2r.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into parameter_services

This commit is contained in:
Martin Dahl 2021-08-29 14:35:18 +02:00
parent 41e5d27ac9 80c0db3f35
commit 1d6cfac7f8
27 changed files with 4985 additions and 3489 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Cargo.toml
View File

@ -1,7 +1,7 @@
[package]
name = "r2r"
version = "0.1.0"
version = "0.3.8"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
description = "Minimal ros2 bindings."
license = "Apache-2.0/MIT"
@ -17,12 +17,14 @@ rcl = { path = "rcl", version = "0.1.0" }
msg_gen = { path = "msg_gen", version = "0.1.0" }
actions = { path = "actions", version = "0.1.0" }
uuid = { version = "0.8", features = ["serde", "v4"] }
retain_mut = "0.1.3"
futures = "0.3.15"
[dev-dependencies]
serde_json = "1.0.62"
futures = "0.3.15"
tokio = { version = "1", features = ["full"] } # for example
tokio = { version = "1", features = ["full"] }
rand = "0.8.4"
[build-dependencies]
common = { path = "common", version = "0.1.0" }

8
README.md
View File

@ -3,9 +3,11 @@ R2R - Minimal ROS2 Rust bindings
Minimal bindings for ROS2 that do *not* require hooking in to the ROS2 build infrastructure -- `cargo build` is all you need. Convenience Rust types are created by calling into the c introspection libraries. This circumvents the ROS2 .msg/.idl pipeline by relying on already generated C code. The convenience types can be ignored when you need to trade convenience for performance, e.g. treating large chunks of data manually. By default, the behavior is to build bindings to the RCL and all message types that can be found in the currently sourced ros environment.
When integration with the colcon build system is desired, a CMakeLists.txt file can be used to limit the generation of bindings to only include specific (idl) dependencies. This is done through additional environment variables. A minimal example is available here: <https://github.com/m-dahl/r2r_minimal_node/>.
When integration with the colcon build system is desired, a CMakeLists.txt file can be used to limit the generation of bindings to only include specific (idl) dependencies. This is done through additional environment variables. A minimal example of the colcon integration is available here: <https://github.com/m-dahl/r2r_minimal_node/>.
Manual is available on github pages <https://sequenceplanner.github.io/r2r/>
Manual is available on github pages <https://sequenceplanner.github.io/r2r/> (documention is lacking though).
These bindings are being written organically when things are needed by me and others so please be aware that the API will change.
How to use
--------------------
@ -24,8 +26,6 @@ cargo run --example subscriber_with_thread
ros2 topic pub /hi std_msgs/msg/String "data: 'Hello, world!'"
```
An example application can be found here <https://github.com/sequenceplanner/r2r-echo>, which can be installed by running cargo install --git https://github.com/sequenceplanner/r2r-echo.
What works?
--------------------
- Up to date with ROS2 ~Dashing~ ~Eloquent~ Foxy Galactic

8
build.rs
View File

@ -118,13 +118,21 @@ fn main() {
}
let untyped_helper = msg_gen::generate_untyped_helper(&msg_list);
let untyped_service_helper = msg_gen::generate_untyped_service_helper(&msg_list);
let untyped_action_helper = msg_gen::generate_untyped_action_helper(&msg_list);
let out_path = PathBuf::from(env::var("OUT_DIR").unwrap());
let msgs_fn = out_path.join("_r2r_generated_msgs.rs");
let untyped_fn = out_path.join("_r2r_generated_untyped_helper.rs");
let untyped_service_fn = out_path.join("_r2r_generated_service_helper.rs");
let untyped_action_fn = out_path.join("_r2r_generated_action_helper.rs");
let mut f = File::create(msgs_fn).unwrap();
write!(f, "{}", modules).unwrap();
let mut f = File::create(untyped_fn).unwrap();
write!(f, "{}", untyped_helper).unwrap();
let mut f = File::create(untyped_service_fn).unwrap();
write!(f, "{}", untyped_service_helper).unwrap();
let mut f = File::create(untyped_action_fn).unwrap();
write!(f, "{}", untyped_action_helper).unwrap();
}

38
examples/action_client.rs
View File

@ -10,34 +10,37 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?;
let client = node.create_action_client::<Fibonacci::Action>("/fibonacci")?;
let action_server_available = node.is_available(&client)?;
// signal that we are done
let done = Arc::new(Mutex::new(false));
println!("waiting for action service...");
while !node.action_server_available(&client)? {
std::thread::sleep(std::time::Duration::from_millis(500));
}
println!("action service available.");
let goal = Fibonacci::Goal { order: 10 };
println!("sending goal: {:?}", goal);
let goal_fut = client.send_goal_request(goal)?;
let mut pool = LocalPool::new();
let spawner = pool.spawner();
let task_spawner = spawner.clone();
let task_done = done.clone();
spawner.spawn_local(async move {
let mut goal = goal_fut.await.unwrap(); // assume success
println!("waiting for action service...");
action_server_available
.await
.expect("could not await action server");
println!("action service available.");
let goal = Fibonacci::Goal { order: 5 };
println!("sending goal: {:?}", goal);
let (goal, result, feedback) = client
.send_goal_request(goal)
.expect("could not send goal request")
.await
.expect("goal rejected by server");
println!("goal accepted: {}", goal.uuid);
// process feedback stream in its own task
let nested_goal = goal.clone();
let nested_task_done = task_done.clone();
task_spawner
.spawn_local(goal.get_feedback().unwrap().for_each(move |msg| {
.spawn_local(feedback.for_each(move |msg| {
let nested_task_done = nested_task_done.clone();
let nested_goal = nested_goal.clone();
async move {
@ -47,8 +50,8 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
nested_goal.get_status()
);
// cancel the goal before it finishes. (comment out to complete the goal)
if msg.sequence.len() == 8 {
// 50/50 that cancel the goal before it finishes.
if msg.sequence.len() == 4 && rand::random::<bool>() {
nested_goal
.cancel()
.unwrap()
@ -64,10 +67,9 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
.unwrap();
// await result in this task
let result = goal.get_result().unwrap().await;
match result {
Ok(msg) => {
println!("got result {:?}", msg);
match result.await {
Ok((status, msg)) => {
println!("got result {} with msg {:?}", status, msg);
*task_done.lock().unwrap() = true;
}
Err(e) => println!("action failed: {:?}", e),

93
examples/action_client_untyped.rs Normal file
View File

@ -0,0 +1,93 @@
//
// This example is the same as action_client.rs but stripped of all (explicit) type information.
//
use futures::executor::LocalPool;
use futures::future::FutureExt;
use futures::stream::StreamExt;
use futures::task::LocalSpawnExt;
use r2r;
use std::sync::{Arc, Mutex};
fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?;
let client =
node.create_action_client_untyped("/fibonacci", "example_interfaces/action/Fibonacci")?;
let action_server_available = node.is_available(&client)?;
// signal that we are done
let done = Arc::new(Mutex::new(false));
let mut pool = LocalPool::new();
let spawner = pool.spawner();
let task_spawner = spawner.clone();
let task_done = done.clone();
spawner.spawn_local(async move {
println!("waiting for action service...");
action_server_available
.await
.expect("could not await action server");
println!("action service available.");
let goal = "{ \"order\": 5 }"; // Fibonacci::Goal { order: 5 };
println!("sending goal: {}", goal);
let (goal, result, feedback) = client
.send_goal_request(serde_json::from_str(goal).expect("json malformed"))
.expect("could not send goal request")
.await
.expect("goal rejected by server");
println!("goal accepted: {}", goal.uuid);
// process feedback stream in its own task
let nested_goal = goal.clone();
let nested_task_done = task_done.clone();
task_spawner
.spawn_local(feedback.for_each(move |msg| {
let nested_task_done = nested_task_done.clone();
let nested_goal = nested_goal.clone();
async move {
if let Ok(msg) = msg {
let sequence = msg
.get("sequence")
.and_then(|s| s.as_array())
.expect("unexpected feedback msg");
println!("new feedback msg {} -- {:?}", msg, nested_goal.get_status());
// 50/50 that cancel the goal before it finishes.
if sequence.len() == 4 && rand::random::<bool>() {
nested_goal
.cancel()
.unwrap()
.map(|r| {
println!("goal cancelled: {:?}", r);
// we are done.
*nested_task_done.lock().unwrap() = true;
})
.await;
}
}
}
}))
.unwrap();
// await result in this task
match result.await {
Ok((status, msg)) => {
println!("got result {} with msg {:?}", status, msg);
*task_done.lock().unwrap() = true;
}
Err(e) => println!("action failed: {:?}", e),
}
})?;
loop {
node.spin_once(std::time::Duration::from_millis(100));
pool.run_until_stalled();
if *done.lock().unwrap() {
break;
}
}
Ok(())
}

169
examples/action_server.rs
View File

@ -1,107 +1,110 @@
use futures::executor::LocalPool;
use futures::executor::{LocalPool, LocalSpawner};
use futures::future::{self, Either};
use futures::stream::{Stream, StreamExt};
use futures::task::LocalSpawnExt;
use r2r;
use r2r::example_interfaces::action::Fibonacci;
use r2r::ServerGoal;
use std::sync::{Arc, Mutex};
// note: cannot be blocking.
fn accept_goal_cb(uuid: &uuid::Uuid, goal: &Fibonacci::Goal) -> bool {
println!(
"Got goal request with order {}, goal id: {}",
goal.order, uuid
);
// reject high orders
goal.order < 100
// main goal handling routine.
async fn run_goal(
node: Arc<Mutex<r2r::Node>>,
g: r2r::ServerGoal<Fibonacci::Action>,
) -> Fibonacci::Result {
let mut timer = node // local timer, will be dropped after this request is processed.
.lock()
.unwrap()
.create_wall_timer(std::time::Duration::from_millis(1000))
.expect("could not create timer");
let mut feedback_msg = Fibonacci::Feedback {
sequence: vec![0, 1],
};
g.publish_feedback(feedback_msg.clone()).expect("fail");
let order = g.goal.order as usize;
for i in 1..order {
feedback_msg
.sequence
.push(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]);
g.publish_feedback(feedback_msg.clone()).expect("fail");
println!("Sending feedback: {:?}", feedback_msg);
timer.tick().await.unwrap();
}
Fibonacci::Result {
sequence: feedback_msg.sequence,
}
}
// note: cannot be blocking.
fn accept_cancel_cb(goal: &ServerGoal<Fibonacci::Action>) -> bool {
println!("Got request to cancel {}", goal.uuid);
// always accept cancel requests
true
async fn fibonacci_server(
spawner: LocalSpawner,
node: Arc<Mutex<r2r::Node>>,
mut requests: impl Stream<Item = r2r::GoalRequest<Fibonacci::Action>> + Unpin,
) {
loop {
match requests.next().await {
Some(req) => {
println!(
"Got goal request with order {}, goal id: {}",
req.goal.order, req.uuid
);
// 1/4 chance that we reject the goal for testing.
if rand::random::<bool>() && rand::random::<bool>() {
println!("rejecting goal");
req.reject().expect("could not reject goal");
continue;
}
let (mut g, mut cancel) = req.accept().expect("could not accept goal");
let goal_fut = spawner
.spawn_local_with_handle(run_goal(node.clone(), g.clone()))
.unwrap();
match future::select(goal_fut, cancel.next()).await {
Either::Left((result, _)) => {
// 50/50 that we succeed or abort
if rand::random::<bool>() {
println!("goal completed!");
g.succeed(result).expect("could not send result");
} else {
println!("goal aborted!");
g.abort(result).expect("could not send result");
}
}
Either::Right((request, _)) => {
if let Some(request) = request {
println!("got cancel request: {}", request.uuid);
request.accept();
}
}
};
}
None => break,
}
}
}
fn main() -> Result<(), Box<dyn std::error::Error>> {
let mut pool = LocalPool::new();
let spawner = pool.spawner();
let task_spawner = spawner.clone();
let ctx = r2r::Context::create()?;
let node = Arc::new(Mutex::new(r2r::Node::create(ctx, "testnode", "")?));
// signal that we are done
let done = Arc::new(Mutex::new(false));
let node_cb = node.clone();
let done_cb = done.clone();
let handle_goal_cb = move |mut g: ServerGoal<Fibonacci::Action>| {
// note that we cannot create the timer here, since we are
// called during spin_once which menas whoever is spinning holds the mutex.
// instead we just set up and immediately start a task.
// also we cannot block which is why we spawn the task
let node_cb = node_cb.clone();
let done_cb = done_cb.clone();
task_spawner
.spawn_local(async move {
let mut timer = node_cb
.lock()
.unwrap()
.create_wall_timer(std::time::Duration::from_millis(1000))
.expect("could not create timer");
let mut feedback_msg = Fibonacci::Feedback {
sequence: vec![0, 1],
};
g.publish_feedback(feedback_msg.clone()).expect("fail");
let order = g.goal.order as usize;
for i in 1..order {
if g.is_cancelling() {
println!("Goal cancelled. quitting");
let result_msg = Fibonacci::Result {
sequence: feedback_msg.sequence,
};
g.cancel(result_msg).expect("could not send cancel request");
// signal stopping of the node
*done_cb.lock().unwrap() = true;
return;
}
feedback_msg
.sequence
.push(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]);
g.publish_feedback(feedback_msg.clone()).expect("fail");
println!("Sending feedback: {:?}", feedback_msg);
timer.tick().await.unwrap();
}
let result_msg = Fibonacci::Result {
sequence: feedback_msg.sequence,
};
g.succeed(result_msg).expect("could not set result");
// signal stopping of the node
*done_cb.lock().unwrap() = true;
})
.expect("could not spawn task");
};
let _server = node
let server_requests = node
.lock()
.unwrap()
.create_action_server::<Fibonacci::Action>(
"/fibonacci",
Box::new(accept_goal_cb),
Box::new(accept_cancel_cb),
Box::new(handle_goal_cb),
)?;
.create_action_server::<Fibonacci::Action>("/fibonacci")?;
let node_cb = node.clone();
spawner
.spawn_local(fibonacci_server(spawner.clone(), node_cb, server_requests))
.unwrap();
loop {
node.lock()
.unwrap()
.spin_once(std::time::Duration::from_millis(100));
pool.run_until_stalled();
if *done.lock().unwrap() {
break;
}
}
Ok(())
}

23
examples/client.rs
View File

@ -1,18 +1,25 @@
use futures::executor::LocalPool;
use futures::task::LocalSpawnExt;
use futures::Future;
use r2r;
use std::io::Write;
use r2r::example_interfaces::srv::AddTwoInts;
async fn requester_task(
node_available: impl Future<Output = r2r::Result<()>>,
c: r2r::Client<AddTwoInts::Service>,
) -> Result<(), Box<dyn std::error::Error>> {
let mut x: i64 = 0;
println!("waiting for service...");
node_available.await?;
println!("service available.");
loop {
let req = AddTwoInts::Request { a: 10 * x, b: x };
print!("{} + {} = ", req.a, req.b);
std::io::stdout().flush()?;
let resp = c.request(&req)?.await?;
println!("{} + {} = {}", req.a, req.b, resp.sum);
println!("{}", resp.sum);
x += 1;
if x == 10 {
@ -27,20 +34,14 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
let mut node = r2r::Node::create(ctx, "testnode", "")?;
let client = node.create_client::<AddTwoInts::Service>("/add_two_ints")?;
// wait for service to be available
println!("waiting for service...");
while !node.service_available(&client)? {
std::thread::sleep(std::time::Duration::from_millis(1000));
}
println!("service available.");
let service_available = node.is_available(&client)?;
let mut pool = LocalPool::new();
let spawner = pool.spawner();
spawner.spawn_local(async move {
match requester_task(client).await {
Ok(()) => println!("exiting"),
match requester_task(service_available, client).await {
Ok(()) => println!("done."),
Err(e) => println!("error: {}", e),
}
})?;

2
examples/publishers.rs
View File

@ -22,7 +22,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
},
..Default::default()
};
let mut native = r2r::WrappedNativeMsg::<Int32>::new();
let mut native = r2r::NativeMsg::<Int32>::new();
native.data = count;
publisher.publish(&to_send).unwrap();

92
examples/service.rs
View File

@ -1,19 +1,93 @@
use futures::executor::LocalPool;
use futures::select;
use futures::stream::StreamExt;
use futures::task::LocalSpawnExt;
use futures::FutureExt;
use r2r;
use r2r::example_interfaces::srv::AddTwoInts;
fn handle_service(request: AddTwoInts::Request) -> AddTwoInts::Response {
println!("request: {} + {}", request.a, request.b);
AddTwoInts::Response {
sum: request.a + request.b,
}
}
///
/// This example demonstrates how we can chain async service calls.
///
/// Run toghtether with the client example.
/// e.g. cargo run --example service
/// and in another terminal cargo run --example client
///
fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?;
node.create_service::<AddTwoInts::Service>("/add_two_ints", Box::new(handle_service))?;
let mut service = node.create_service::<AddTwoInts::Service>("/add_two_ints")?;
let service_delayed = node.create_service::<AddTwoInts::Service>("/add_two_ints_delayed")?;
let client = node.create_client::<AddTwoInts::Service>("/add_two_ints_delayed")?;
let mut timer = node.create_wall_timer(std::time::Duration::from_millis(250))?;
let mut timer2 = node.create_wall_timer(std::time::Duration::from_millis(2000))?;
// wait for service to be available
let service_available = node.is_available(&client)?;
let mut pool = LocalPool::new();
let spawner = pool.spawner();
spawner.spawn_local(async move {
println!("waiting for delayed service...");
service_available.await.expect("could not await service");
println!("delayed service available.");
loop {
match service.next().await {
Some(req) => {
println!("passing request on to delayed service");
let resp = client
.request(&req.message)
.expect("could not send client response")
.await
.expect("expected client response");
println!(
"responding with: {} + {} = {}",
req.message.a, req.message.b, resp.sum
);
req.respond(resp).expect("could not send service response");
}
None => break,
}
}
})?;
spawner.spawn_local(async move {
// we need to fuse the streams for select
let mut service_delayed = service_delayed.fuse();
loop {
select! {
req = service_delayed.next() => {
if let Some(req) = req {
let resp = AddTwoInts::Response {
sum: req.message.a + req.message.b,
};
// wait a bit before answering...
let _ret = timer2.tick().await;
req.respond(resp).expect("could not send service response");
}
},
elapsed = timer2.tick().fuse() => {
if let Ok(elapsed) = elapsed {
println!("no request made in {}ms", elapsed.as_millis());
}
}
};
}
})?;
spawner.spawn_local(async move {
loop {
let elapsed = timer.tick().await.expect("could not tick");
println!(
"doing other async work, {}ms since last call",
elapsed.as_millis()
);
}
})?;
loop {
node.spin_once(std::time::Duration::from_millis(100));
node.spin_once(std::time::Duration::from_millis(5));
pool.run_until_stalled();
}
}

4
examples/subscriber.rs
View File

@ -33,10 +33,10 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
})?;
// for sub2 we just print the data
let sub2 = node.subscribe::<r2r::std_msgs::msg::String>("/topic2")?;
let sub2 = node.subscribe_untyped("/topic2", "std_msgs/msg/String")?;
spawner.spawn_local(async move {
sub2.for_each(|msg| {
println!("topic2: new msg: {}", msg.data);
println!("topic2: new msg: {}", msg.expect("deserialization error"));
future::ready(())
})
.await

50
examples/untyped_client.rs Normal file
View File

@ -0,0 +1,50 @@
use futures::executor::LocalPool;
use futures::task::LocalSpawnExt;
use futures::Future;
use r2r;
async fn requester_task(
node_available: impl Future<Output = r2r::Result<()>>,
c: r2r::UntypedClient,
) -> Result<(), Box<dyn std::error::Error>> {
let mut x: i64 = 0;
println!("waiting for service...");
node_available.await?;
println!("service available.");
loop {
let json = format!("{{ \"a\": {}, \"b\": {} }}", 10 * x, x);
let req = serde_json::from_str(&json).unwrap();
let resp = c.request(req)?.await?;
println!("{}", resp.expect("deserialization error"));
x += 1;
if x == 10 {
break;
}
}
Ok(())
}
fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?;
let client =
node.create_client_untyped("/add_two_ints", "example_interfaces/srv/AddTwoInts")?;
let service_available = node.is_available(&client)?;
let mut pool = LocalPool::new();
let spawner = pool.spawner();
spawner.spawn_local(async move {
match requester_task(service_available, client).await {
Ok(()) => println!("done."),
Err(e) => println!("error: {}", e),
}
})?;
loop {
node.spin_once(std::time::Duration::from_millis(100));
pool.run_until_stalled();
}
}

121
msg_gen/src/lib.rs
View File

@ -233,8 +233,16 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String {
let mut fields = String::new();
let is_empty_msg = members.len() == 1
&& field_name(CStr::from_ptr(members[0].name_).to_str().unwrap())
== "structure_needs_at_least_one_member";
for member in members {
let field_name = field_name(CStr::from_ptr(member.name_).to_str().unwrap());
if field_name == "structure_needs_at_least_one_member" {
// Yay we can have empty structs in rust
continue;
}
let rust_field_type = field_type(member.type_id_);
let rust_field_type = if rust_field_type == "message" {
let (module, prefix, name, _, _) = introspection(member.members_);
@ -283,14 +291,25 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String {
}
let mut from_native = String::new();
from_native.push_str(&format!(
"fn from_native(msg: &Self::CStruct) -> {} {{\n",
name
));
if is_empty_msg {
from_native.push_str(&format!(
"fn from_native(_msg: &Self::CStruct) -> {} {{\n",
name
));
} else {
from_native.push_str(&format!(
"fn from_native(msg: &Self::CStruct) -> {} {{\n",
name
));
}
from_native.push_str(&format!(" {} {{\n", name));
for member in members {
let field_name = field_name(CStr::from_ptr(member.name_).to_str().unwrap());
if field_name == "structure_needs_at_least_one_member" {
// Yay we can have empty structs in rust
continue;
}
let rust_field_type = field_type(member.type_id_);
if member.is_array_ && member.array_size_ > 0 && !member.is_upper_bound_ {
@ -368,10 +387,18 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String {
from_native.push_str(" }\n }\n");
let mut copy_to_native = String::new();
copy_to_native.push_str("fn copy_to_native(&self, msg: &mut Self::CStruct) {");
if is_empty_msg {
copy_to_native.push_str("fn copy_to_native(&self, _msg: &mut Self::CStruct) {");
} else {
copy_to_native.push_str("fn copy_to_native(&self, msg: &mut Self::CStruct) {");
}
for member in members {
let field_name = field_name(CStr::from_ptr((*member).name_).to_str().unwrap());
if field_name == "structure_needs_at_least_one_member" {
// Yay we can have empty structs in rust
continue;
}
let rust_field_type = field_type(member.type_id_);
if member.is_array_ && member.array_size_ > 0 && !member.is_upper_bound_ {
@ -486,7 +513,7 @@ pub fn generate_untyped_helper(msgs: &Vec<common::RosMsg>) -> String {
let open = String::from(
"
impl WrappedNativeMsgUntyped {
fn new_from(typename: &str) -> Result<Self> {
pub fn new_from(typename: &str) -> Result<Self> {
",
);
let close = String::from(
@ -523,3 +550,85 @@ impl WrappedNativeMsgUntyped {
format!("{}{}{}", open, lines, close)
}
pub fn generate_untyped_service_helper(msgs: &Vec<common::RosMsg>) -> String {
let open = String::from(
"
impl UntypedServiceSupport {
pub fn new_from(typename: &str) -> Result<Self> {
",
);
let close = String::from(
"
else
{
return Err(Error::InvalidMessageType{ msgtype: typename.into() })
}
}
}
",
);
let mut lines = String::new();
for msg in msgs {
if msg.prefix != "srv" {
continue;
}
let typename = format!("{}/{}/{}", msg.module, msg.prefix, msg.name);
let rustname = format!("{}::{}::{}::Service", msg.module, msg.prefix, msg.name);
lines.push_str(&format!(
"
if typename == \"{typename}\" {{
return Ok(UntypedServiceSupport::new::<{rustname}>());
}}
",
typename = typename,
rustname = rustname
));
}
format!("{}{}{}", open, lines, close)
}
pub fn generate_untyped_action_helper(msgs: &Vec<common::RosMsg>) -> String {
let open = String::from(
"
impl UntypedActionSupport {
pub fn new_from(typename: &str) -> Result<Self> {
",
);
let close = String::from(
"
else
{
return Err(Error::InvalidMessageType{ msgtype: typename.into() })
}
}
}
",
);
let mut lines = String::new();
for msg in msgs {
if msg.prefix != "action" {
continue;
}
let typename = format!("{}/{}/{}", msg.module, msg.prefix, msg.name);
let rustname = format!("{}::{}::{}::Action", msg.module, msg.prefix, msg.name);
lines.push_str(&format!(
"
if typename == \"{typename}\" {{
return Ok(UntypedActionSupport::new::<{rustname}>());
}}
",
typename = typename,
rustname = rustname
));
}
format!("{}{}{}", open, lines, close)
}

4
rcl/src/lib.rs
View File

@ -140,10 +140,10 @@ macro_rules! primitive_sequence {
primitive_sequence!(rosidl_runtime_c__float32, f32);
primitive_sequence!(rosidl_runtime_c__float64, f64);
#[cfg(target_arch = "aarch64")]
#[cfg(all(target_os = "macos", target_arch = "aarch64"))]
primitive_sequence!(rosidl_runtime_c__long_double, f64);
#[cfg(not(target_arch = "aarch64"))]
#[cfg(not(all(target_os = "macos", target_arch = "aarch64")))]
primitive_sequence!(rosidl_runtime_c__long_double, u128);
primitive_sequence!(rosidl_runtime_c__char, i8);

612
src/action_clients.rs Normal file
View File

@ -0,0 +1,612 @@
use super::*;
unsafe impl<T> Send for ActionClient<T> where T: WrappedActionTypeSupport {}
#[derive(Clone)]
pub struct ActionClient<T>
where
T: WrappedActionTypeSupport,
{
client: Weak<Mutex<WrappedActionClient<T>>>,
}
unsafe impl<T> Send for ClientGoal<T> where T: WrappedActionTypeSupport {}
#[derive(Clone)]
pub struct ClientGoal<T>
where
T: WrappedActionTypeSupport,
{
client: Weak<Mutex<WrappedActionClient<T>>>,
pub uuid: uuid::Uuid,
}
impl<T: 'static> ClientGoal<T>
where
T: WrappedActionTypeSupport,
{
pub fn get_status(&self) -> Result<GoalStatus> {
let client = self
.client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let client = client.lock().unwrap();
Ok(client.get_goal_status(&self.uuid))
}
pub fn cancel(&self) -> Result<impl Future<Output = Result<()>>> {
// upgrade to actual ref. if still alive
let client = self
.client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
client.send_cancel_request(&self.uuid)
}
}
impl<T: 'static> ActionClient<T>
where
T: WrappedActionTypeSupport,
{
pub fn send_goal_request(
&self,
goal: T::Goal,
) -> Result<
impl Future<
Output = Result<(
ClientGoal<T>,
impl Future<Output = Result<(GoalStatus, T::Result)>>,
impl Stream<Item = T::Feedback> + Unpin,
)>,
>,
>
where
T: WrappedActionTypeSupport,
{
// upgrade to actual ref. if still alive
let client = self
.client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
let uuid = uuid::Uuid::new_v4();
let uuid_msg = unique_identifier_msgs::msg::UUID {
uuid: uuid.as_bytes().to_vec(),
};
let request_msg = T::make_goal_request_msg(uuid_msg, goal);
let native_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request,
>::from(&request_msg);
let mut seq_no = 0i64;
let result = unsafe {
rcl_action_send_goal_request(&client.rcl_handle, native_msg.void_ptr(), &mut seq_no)
};
// set up channels
let (goal_req_sender, goal_req_receiver) =
oneshot::channel::<(bool, builtin_interfaces::msg::Time)>();
let (feedback_sender, feedback_receiver) = mpsc::channel::<T::Feedback>(10);
client.feedback_senders.push((uuid, feedback_sender));
let (result_sender, result_receiver) = oneshot::channel::<(GoalStatus, T::Result)>();
client.result_senders.push((uuid, result_sender));
if result == RCL_RET_OK as i32 {
client
.goal_response_channels
.push((seq_no, goal_req_sender));
// instead of "canceled" we return invalid client.
let fut_client = Weak::clone(&self.client);
let future = goal_req_receiver
.map_err(|_| Error::RCL_RET_ACTION_CLIENT_INVALID)
.map(move |r| match r {
Ok((accepted, _stamp)) => {
if accepted {
// on goal accept we immediately send the result request
{
let c = fut_client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let mut c = c.lock().unwrap();
c.send_result_request(uuid.clone());
}
Ok((
ClientGoal {
client: fut_client,
uuid,
},
result_receiver.map_err(|_| Error::RCL_RET_ACTION_CLIENT_INVALID),
feedback_receiver,
))
} else {
Err(Error::RCL_RET_ACTION_GOAL_REJECTED)
}
}
Err(e) => Err(e),
});
Ok(future)
} else {
eprintln!("coult not send goal request {}", result);
Err(Error::from_rcl_error(result))
}
}
}
pub fn make_action_client<T>(client: Weak<Mutex<WrappedActionClient<T>>>) -> ActionClient<T>
where
T: WrappedActionTypeSupport,
{
ActionClient { client }
}
#[derive(Debug, Copy, Clone, PartialEq)]
pub enum GoalStatus {
Unknown,
Accepted,
Executing,
Canceling,
Succeeded,
Canceled,
Aborted,
}
impl GoalStatus {
#[allow(dead_code)]
pub fn to_rcl(&self) -> i8 {
match self {
GoalStatus::Unknown => 0,
GoalStatus::Accepted => 1,
GoalStatus::Executing => 2,
GoalStatus::Canceling => 3,
GoalStatus::Succeeded => 4,
GoalStatus::Canceled => 5,
GoalStatus::Aborted => 6,
}
}
pub fn from_rcl(s: i8) -> Self {
match s {
0 => GoalStatus::Unknown,
1 => GoalStatus::Accepted,
2 => GoalStatus::Executing,
3 => GoalStatus::Canceling,
4 => GoalStatus::Succeeded,
5 => GoalStatus::Canceled,
6 => GoalStatus::Aborted,
_ => panic!("unknown action status: {}", s),
}
}
}
impl std::fmt::Display for GoalStatus {
fn fmt(&self, fmtr: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
let s = match self {
GoalStatus::Unknown => "unknown",
GoalStatus::Accepted => "accepted",
GoalStatus::Executing => "executing",
GoalStatus::Canceling => "canceling",
GoalStatus::Succeeded => "succeeded",
GoalStatus::Canceled => "canceled",
GoalStatus::Aborted => "aborted",
};
write!(fmtr, "{}", s)
}
}
pub struct WrappedActionClient<T>
where
T: WrappedActionTypeSupport,
{
pub rcl_handle: rcl_action_client_t,
pub goal_response_channels: Vec<(i64, oneshot::Sender<(bool, builtin_interfaces::msg::Time)>)>,
pub cancel_response_channels:
Vec<(i64, oneshot::Sender<action_msgs::srv::CancelGoal::Response>)>,
pub feedback_senders: Vec<(uuid::Uuid, mpsc::Sender<T::Feedback>)>,
pub result_requests: Vec<(i64, uuid::Uuid)>,
pub result_senders: Vec<(uuid::Uuid, oneshot::Sender<(GoalStatus, T::Result)>)>,
pub goal_status: HashMap<uuid::Uuid, GoalStatus>,
pub poll_available_channels: Vec<oneshot::Sender<()>>,
}
pub trait ActionClient_ {
fn handle(&self) -> &rcl_action_client_t;
fn destroy(&mut self, node: &mut rcl_node_t) -> ();
fn handle_goal_response(&mut self) -> ();
fn handle_cancel_response(&mut self) -> ();
fn handle_feedback_msg(&mut self) -> ();
fn handle_status_msg(&mut self) -> ();
fn handle_result_response(&mut self) -> ();
fn send_result_request(&mut self, uuid: uuid::Uuid) -> ();
fn register_poll_available(&mut self, s: oneshot::Sender<()>) -> ();
fn poll_available(&mut self, node: &mut rcl_node_t) -> ();
}
use std::convert::TryInto;
pub fn vec_to_uuid_bytes<T>(v: Vec<T>) -> [T; 16] {
v.try_into().unwrap_or_else(|v: Vec<T>| {
panic!("Expected a Vec of length {} but it was {}", 16, v.len())
})
}
impl<T> WrappedActionClient<T>
where
T: WrappedActionTypeSupport,
{
pub fn get_goal_status(&self, uuid: &uuid::Uuid) -> GoalStatus {
*self.goal_status.get(uuid).unwrap_or(&GoalStatus::Unknown)
}
pub fn send_cancel_request(
&mut self,
goal: &uuid::Uuid,
) -> Result<impl Future<Output = Result<()>>>
where
T: WrappedActionTypeSupport,
{
let msg = action_msgs::srv::CancelGoal::Request {
goal_info: action_msgs::msg::GoalInfo {
goal_id: unique_identifier_msgs::msg::UUID {
uuid: goal.as_bytes().to_vec(),
},
..action_msgs::msg::GoalInfo::default()
},
};
let native_msg = WrappedNativeMsg::<action_msgs::srv::CancelGoal::Request>::from(&msg);
let mut seq_no = 0i64;
let result = unsafe {
rcl_action_send_cancel_request(&self.rcl_handle, native_msg.void_ptr(), &mut seq_no)
};
if result == RCL_RET_OK as i32 {
let (cancel_req_sender, cancel_req_receiver) =
oneshot::channel::<action_msgs::srv::CancelGoal::Response>();
self.cancel_response_channels
.push((seq_no, cancel_req_sender));
// instead of "canceled" we return invalid client.
let future = cancel_req_receiver
.map_err(|_| Error::RCL_RET_CLIENT_INVALID)
.map(|r| match r {
Ok(r) => match r.return_code {
0 => Ok(()),
1 => Err(Error::GoalCancelRejected),
2 => Err(Error::GoalCancelUnknownGoalID),
3 => Err(Error::GoalCancelAlreadyTerminated),
x => panic!("unknown error code return from action server: {}", x),
},
Err(e) => Err(e),
});
Ok(future)
} else {
eprintln!("coult not send goal request {}", result);
Err(Error::from_rcl_error(result))
}
}
}
impl<T: 'static> ActionClient_ for WrappedActionClient<T>
where
T: WrappedActionTypeSupport,
{
fn handle(&self) -> &rcl_action_client_t {
&self.rcl_handle
}
fn handle_goal_response(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut response_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
>::new();
let ret = unsafe {
rcl_action_take_goal_response(
&self.rcl_handle,
request_id.as_mut_ptr(),
response_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
if let Some(idx) = self
.goal_response_channels
.iter()
.position(|(id, _)| id == &request_id.sequence_number)
{
let (_, sender) = self.goal_response_channels.swap_remove(idx);
let response = <<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response::from_native(&response_msg);
let (accept, stamp) = T::destructure_goal_response_msg(response);
match sender.send((accept, stamp)) {
Ok(()) => {}
Err(e) => {
println!("error sending to action client: {:?}", e);
}
}
} else {
let we_have: String = self
.goal_response_channels
.iter()
.map(|(id, _)| id.to_string())
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
request_id.sequence_number, we_have
);
}
}
}
fn handle_cancel_response(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut response_msg = WrappedNativeMsg::<action_msgs::srv::CancelGoal::Response>::new();
let ret = unsafe {
rcl_action_take_cancel_response(
&self.rcl_handle,
request_id.as_mut_ptr(),
response_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
if let Some(idx) = self
.cancel_response_channels
.iter()
.position(|(id, _)| id == &request_id.sequence_number)
{
let (_, sender) = self.cancel_response_channels.swap_remove(idx);
let response = action_msgs::srv::CancelGoal::Response::from_native(&response_msg);
match sender.send(response) {
Err(e) => eprintln!("warning: could not send cancel response msg ({:?})", e),
_ => (),
}
} else {
let we_have: String = self
.goal_response_channels
.iter()
.map(|(id, _)| id.to_string())
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
request_id.sequence_number, we_have
);
}
}
}
fn handle_feedback_msg(&mut self) -> () {
let mut feedback_msg = WrappedNativeMsg::<T::FeedbackMessage>::new();
let ret =
unsafe { rcl_action_take_feedback(&self.rcl_handle, feedback_msg.void_ptr_mut()) };
if ret == RCL_RET_OK as i32 {
let msg = T::FeedbackMessage::from_native(&feedback_msg);
let (uuid, feedback) = T::destructure_feedback_msg(msg);
let msg_uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(uuid.uuid));
if let Some((_, sender)) = self
.feedback_senders
.iter_mut()
.find(|(uuid, _)| uuid == &msg_uuid)
{
match sender.try_send(feedback) {
Err(e) => eprintln!("warning: could not send feedback msg ({})", e),
_ => (),
}
}
}
}
fn handle_status_msg(&mut self) -> () {
let mut status_array = WrappedNativeMsg::<action_msgs::msg::GoalStatusArray>::new();
let ret = unsafe { rcl_action_take_status(&self.rcl_handle, status_array.void_ptr_mut()) };
if ret == RCL_RET_OK as i32 {
let arr = action_msgs::msg::GoalStatusArray::from_native(&status_array);
for a in &arr.status_list {
let uuid =
uuid::Uuid::from_bytes(vec_to_uuid_bytes(a.goal_info.goal_id.uuid.clone()));
if !self.result_senders.iter().any(|(suuid, _)| suuid == &uuid) {
continue;
}
let status = GoalStatus::from_rcl(a.status);
*self.goal_status.entry(uuid).or_insert(GoalStatus::Unknown) = status;
}
}
}
fn handle_result_response(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut response_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
>::new();
let ret = unsafe {
rcl_action_take_result_response(
&self.rcl_handle,
request_id.as_mut_ptr(),
response_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
if let Some(idx) = self
.result_requests
.iter()
.position(|(id, _)| id == &request_id.sequence_number)
{
let (_, uuid) = self.result_requests.swap_remove(idx);
if let Some(idx) = self
.result_senders
.iter()
.position(|(suuid, _)| suuid == &uuid)
{
let (_, sender) = self.result_senders.swap_remove(idx);
let response = <<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response::from_native(&response_msg);
let (status, result) = T::destructure_result_response_msg(response);
let status = GoalStatus::from_rcl(status);
match sender.send((status, result)) {
Ok(()) => {}
Err(e) => {
println!("error sending result to action client: {:?}", e);
}
}
}
} else {
let we_have: String = self
.result_requests
.iter()
.map(|(id, _)| id.to_string())
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
request_id.sequence_number, we_have
);
}
}
}
fn send_result_request(&mut self, uuid: uuid::Uuid) -> () {
let uuid_msg = unique_identifier_msgs::msg::UUID {
uuid: uuid.as_bytes().to_vec(),
};
let request_msg = T::make_result_request_msg(uuid_msg);
let native_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request,
>::from(&request_msg);
let mut seq_no = 0i64;
let result = unsafe {
rcl_action_send_result_request(&self.rcl_handle, native_msg.void_ptr(), &mut seq_no)
};
if result == RCL_RET_OK as i32 {
self.result_requests.push((seq_no, uuid));
} else {
eprintln!("coult not send request {}", result);
}
}
fn register_poll_available(&mut self, s: oneshot::Sender<()>) {
self.poll_available_channels.push(s);
}
fn poll_available(&mut self, node: &mut rcl_node_t) {
if self.poll_available_channels.is_empty() {
return;
}
let available = action_server_available_helper(node, self.handle());
match available {
Ok(true) => {
// send ok and close channels
while let Some(sender) = self.poll_available_channels.pop() {
let _res = sender.send(()); // we ignore if receiver dropped.
}
}
Ok(false) => {
// not available...
}
Err(_) => {
// error, close all channels
self.poll_available_channels.clear();
}
}
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_action_client_fini(&mut self.rcl_handle, node);
}
}
}
pub fn create_action_client_helper(
node: &mut rcl_node_t,
action_name: &str,
action_ts: *const rosidl_action_type_support_t,
) -> Result<rcl_action_client_t> {
let mut client_handle = unsafe { rcl_action_get_zero_initialized_client() };
let action_name_c_string =
CString::new(action_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
let result = unsafe {
let client_options = rcl_action_client_get_default_options();
rcl_action_client_init(
&mut client_handle,
node,
action_ts,
action_name_c_string.as_ptr(),
&client_options,
)
};
if result == RCL_RET_OK as i32 {
Ok(client_handle)
} else {
Err(Error::from_rcl_error(result))
}
}
pub fn action_client_get_num_waits(
rcl_handle: &rcl_action_client_t,
num_subs: &mut usize,
num_gc: &mut usize,
num_timers: &mut usize,
num_clients: &mut usize,
num_services: &mut usize,
) -> Result<()> {
unsafe {
let result = rcl_action_client_wait_set_get_num_entities(
rcl_handle,
num_subs,
num_gc,
num_timers,
num_clients,
num_services,
);
if result == RCL_RET_OK as i32 {
Ok(())
} else {
Err(Error::from_rcl_error(result))
}
}
}
use crate::nodes::IsAvailablePollable;
impl<T: 'static> IsAvailablePollable for ActionClient<T>
where
T: WrappedActionTypeSupport,
{
fn register_poll_available(&self, sender: oneshot::Sender<()>) -> Result<()> {
let client = self
.client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
client.register_poll_available(sender);
Ok(())
}
}
pub fn action_server_available_helper(
node: &rcl_node_t,
client: &rcl_action_client_t,
) -> Result<bool> {
let mut avail = false;
let result = unsafe { rcl_action_server_is_available(node, client, &mut avail) };
if result == RCL_RET_OK as i32 {
Ok(avail)
} else {
eprintln!("coult not check if action server is available {}", result);
Err(Error::from_rcl_error(result))
}
}

437
src/action_clients_untyped.rs Normal file
View File

@ -0,0 +1,437 @@
use super::*;
//
// TODO: refactor this to separate out shared code between action client and this.
//
unsafe impl Send for ActionClientUntyped {}
#[derive(Clone)]
pub struct ActionClientUntyped {
client: Weak<Mutex<WrappedActionClientUntyped>>,
}
unsafe impl Send for ClientGoalUntyped {}
#[derive(Clone)]
pub struct ClientGoalUntyped {
client: Weak<Mutex<WrappedActionClientUntyped>>,
pub uuid: uuid::Uuid,
}
impl ClientGoalUntyped {
pub fn get_status(&self) -> Result<GoalStatus> {
let client = self
.client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let client = client.lock().unwrap();
Ok(client.get_goal_status(&self.uuid))
}
pub fn cancel(&self) -> Result<impl Future<Output = Result<()>>> {
// upgrade to actual ref. if still alive
let client = self
.client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
client.send_cancel_request(&self.uuid)
}
}
impl ActionClientUntyped {
pub fn send_goal_request(
&self,
goal: serde_json::Value, // T::Goal
) -> Result<
impl Future<
Output = Result<(
ClientGoalUntyped,
impl Future<Output = Result<(GoalStatus, Result<serde_json::Value>)>>, // T::Result
impl Stream<Item = Result<serde_json::Value>> + Unpin, // T::Feedback
)>,
>,
> {
// upgrade to actual ref. if still alive
let client = self
.client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
let uuid = uuid::Uuid::new_v4();
let uuid_msg = unique_identifier_msgs::msg::UUID {
uuid: uuid.as_bytes().to_vec(),
};
let native_msg = (client.action_type_support.make_goal_request_msg)(uuid_msg, goal);
let mut seq_no = 0i64;
let result = unsafe {
rcl_action_send_goal_request(&client.rcl_handle, native_msg.void_ptr(), &mut seq_no)
};
// set up channels
let (goal_req_sender, goal_req_receiver) =
oneshot::channel::<(bool, builtin_interfaces::msg::Time)>();
let (feedback_sender, feedback_receiver) = mpsc::channel::<Result<serde_json::Value>>(10);
client.feedback_senders.push((uuid, feedback_sender));
let (result_sender, result_receiver) =
oneshot::channel::<(GoalStatus, Result<serde_json::Value>)>();
client.result_senders.push((uuid, result_sender));
if result == RCL_RET_OK as i32 {
client
.goal_response_channels
.push((seq_no, goal_req_sender));
// instead of "canceled" we return invalid client.
let fut_client = Weak::clone(&self.client);
let future = goal_req_receiver
.map_err(|_| Error::RCL_RET_ACTION_CLIENT_INVALID)
.map(move |r| match r {
Ok((accepted, _stamp)) => {
if accepted {
// on goal accept we immediately send the result request
{
let c = fut_client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let mut c = c.lock().unwrap();
c.send_result_request(uuid.clone());
}
Ok((
ClientGoalUntyped {
client: fut_client,
uuid,
},
result_receiver.map_err(|_| Error::RCL_RET_ACTION_CLIENT_INVALID),
feedback_receiver,
))
} else {
Err(Error::RCL_RET_ACTION_GOAL_REJECTED)
}
}
Err(e) => Err(e),
});
Ok(future)
} else {
eprintln!("coult not send goal request {}", result);
Err(Error::from_rcl_error(result))
}
}
}
pub fn make_action_client_untyped(
client: Weak<Mutex<WrappedActionClientUntyped>>,
) -> ActionClientUntyped {
ActionClientUntyped { client }
}
pub struct WrappedActionClientUntyped {
pub action_type_support: UntypedActionSupport,
pub rcl_handle: rcl_action_client_t,
pub goal_response_channels: Vec<(i64, oneshot::Sender<(bool, builtin_interfaces::msg::Time)>)>,
pub cancel_response_channels:
Vec<(i64, oneshot::Sender<action_msgs::srv::CancelGoal::Response>)>,
pub feedback_senders: Vec<(uuid::Uuid, mpsc::Sender<Result<serde_json::Value>>)>,
pub result_requests: Vec<(i64, uuid::Uuid)>,
pub result_senders: Vec<(
uuid::Uuid,
oneshot::Sender<(GoalStatus, Result<serde_json::Value>)>,
)>,
pub goal_status: HashMap<uuid::Uuid, GoalStatus>,
pub poll_available_channels: Vec<oneshot::Sender<()>>,
}
impl WrappedActionClientUntyped {
pub fn get_goal_status(&self, uuid: &uuid::Uuid) -> GoalStatus {
*self.goal_status.get(uuid).unwrap_or(&GoalStatus::Unknown)
}
pub fn send_cancel_request(
&mut self,
goal: &uuid::Uuid,
) -> Result<impl Future<Output = Result<()>>> {
let msg = action_msgs::srv::CancelGoal::Request {
goal_info: action_msgs::msg::GoalInfo {
goal_id: unique_identifier_msgs::msg::UUID {
uuid: goal.as_bytes().to_vec(),
},
..action_msgs::msg::GoalInfo::default()
},
};
let native_msg = WrappedNativeMsg::<action_msgs::srv::CancelGoal::Request>::from(&msg);
let mut seq_no = 0i64;
let result = unsafe {
rcl_action_send_cancel_request(&self.rcl_handle, native_msg.void_ptr(), &mut seq_no)
};
if result == RCL_RET_OK as i32 {
let (cancel_req_sender, cancel_req_receiver) =
oneshot::channel::<action_msgs::srv::CancelGoal::Response>();
self.cancel_response_channels
.push((seq_no, cancel_req_sender));
// instead of "canceled" we return invalid client.
let future = cancel_req_receiver
.map_err(|_| Error::RCL_RET_CLIENT_INVALID)
.map(|r| match r {
Ok(r) => match r.return_code {
0 => Ok(()),
1 => Err(Error::GoalCancelRejected),
2 => Err(Error::GoalCancelUnknownGoalID),
3 => Err(Error::GoalCancelAlreadyTerminated),
x => panic!("unknown error code return from action server: {}", x),
},
Err(e) => Err(e),
});
Ok(future)
} else {
eprintln!("coult not send goal request {}", result);
Err(Error::from_rcl_error(result))
}
}
}
impl ActionClient_ for WrappedActionClientUntyped {
fn handle(&self) -> &rcl_action_client_t {
&self.rcl_handle
}
fn handle_goal_response(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut response_msg = (self.action_type_support.make_goal_response_msg)();
let ret = unsafe {
rcl_action_take_goal_response(
&self.rcl_handle,
request_id.as_mut_ptr(),
response_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
if let Some(idx) = self
.goal_response_channels
.iter()
.position(|(id, _)| id == &request_id.sequence_number)
{
let (_, sender) = self.goal_response_channels.swap_remove(idx);
let (accept, stamp) =
(self.action_type_support.destructure_goal_response_msg)(response_msg);
match sender.send((accept, stamp)) {
Ok(()) => {}
Err(e) => {
println!("error sending to action client: {:?}", e);
}
}
} else {
let we_have: String = self
.goal_response_channels
.iter()
.map(|(id, _)| id.to_string())
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
request_id.sequence_number, we_have
);
}
}
}
fn handle_cancel_response(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut response_msg = WrappedNativeMsg::<action_msgs::srv::CancelGoal::Response>::new();
let ret = unsafe {
rcl_action_take_cancel_response(
&self.rcl_handle,
request_id.as_mut_ptr(),
response_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
if let Some(idx) = self
.cancel_response_channels
.iter()
.position(|(id, _)| id == &request_id.sequence_number)
{
let (_, sender) = self.cancel_response_channels.swap_remove(idx);
let response = action_msgs::srv::CancelGoal::Response::from_native(&response_msg);
match sender.send(response) {
Err(e) => eprintln!("warning: could not send cancel response msg ({:?})", e),
_ => (),
}
} else {
let we_have: String = self
.goal_response_channels
.iter()
.map(|(id, _)| id.to_string())
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
request_id.sequence_number, we_have
);
}
}
}
fn handle_feedback_msg(&mut self) -> () {
let mut feedback_msg = (self.action_type_support.make_feedback_msg)();
let ret =
unsafe { rcl_action_take_feedback(&self.rcl_handle, feedback_msg.void_ptr_mut()) };
if ret == RCL_RET_OK as i32 {
let (uuid, feedback) =
(self.action_type_support.destructure_feedback_msg)(feedback_msg);
let msg_uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(uuid.uuid));
if let Some((_, sender)) = self
.feedback_senders
.iter_mut()
.find(|(uuid, _)| uuid == &msg_uuid)
{
match sender.try_send(feedback) {
Err(e) => eprintln!("warning: could not send feedback msg ({})", e),
_ => (),
}
}
}
}
fn handle_status_msg(&mut self) -> () {
let mut status_array = WrappedNativeMsg::<action_msgs::msg::GoalStatusArray>::new();
let ret = unsafe { rcl_action_take_status(&self.rcl_handle, status_array.void_ptr_mut()) };
if ret == RCL_RET_OK as i32 {
let arr = action_msgs::msg::GoalStatusArray::from_native(&status_array);
for a in &arr.status_list {
let uuid =
uuid::Uuid::from_bytes(vec_to_uuid_bytes(a.goal_info.goal_id.uuid.clone()));
if !self.result_senders.iter().any(|(suuid, _)| suuid == &uuid) {
continue;
}
let status = GoalStatus::from_rcl(a.status);
*self.goal_status.entry(uuid).or_insert(GoalStatus::Unknown) = status;
}
}
}
fn handle_result_response(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut response_msg = (self.action_type_support.make_result_response_msg)();
let ret = unsafe {
rcl_action_take_result_response(
&self.rcl_handle,
request_id.as_mut_ptr(),
response_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
if let Some(idx) = self
.result_requests
.iter()
.position(|(id, _)| id == &request_id.sequence_number)
{
let (_, uuid) = self.result_requests.swap_remove(idx);
if let Some(idx) = self
.result_senders
.iter()
.position(|(suuid, _)| suuid == &uuid)
{
let (_, sender) = self.result_senders.swap_remove(idx);
let (status, result) =
(self.action_type_support.destructure_result_response_msg)(response_msg);
let status = GoalStatus::from_rcl(status);
match sender.send((status, result)) {
Ok(()) => {}
Err(e) => {
println!("error sending result to action client: {:?}", e);
}
}
}
} else {
let we_have: String = self
.result_requests
.iter()
.map(|(id, _)| id.to_string())
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
request_id.sequence_number, we_have
);
}
}
}
fn send_result_request(&mut self, uuid: uuid::Uuid) -> () {
let uuid_msg = unique_identifier_msgs::msg::UUID {
uuid: uuid.as_bytes().to_vec(),
};
let native_msg = (self.action_type_support.make_result_request_msg)(uuid_msg);
let mut seq_no = 0i64;
let result = unsafe {
rcl_action_send_result_request(&self.rcl_handle, native_msg.void_ptr(), &mut seq_no)
};
if result == RCL_RET_OK as i32 {
self.result_requests.push((seq_no, uuid));
} else {
eprintln!("coult not send request {}", result);
}
}
fn register_poll_available(&mut self, s: oneshot::Sender<()>) {
self.poll_available_channels.push(s);
}
fn poll_available(&mut self, node: &mut rcl_node_t) {
if self.poll_available_channels.is_empty() {
return;
}
let available = action_server_available_helper(node, self.handle());
match available {
Ok(true) => {
// send ok and close channels
while let Some(sender) = self.poll_available_channels.pop() {
let _res = sender.send(()); // we ignore if receiver dropped.
}
}
Ok(false) => {
// not available...
}
Err(_) => {
// error, close all channels
self.poll_available_channels.clear();
}
}
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_action_client_fini(&mut self.rcl_handle, node);
}
}
}
use crate::nodes::IsAvailablePollable;
impl IsAvailablePollable for ActionClientUntyped {
fn register_poll_available(&self, sender: oneshot::Sender<()>) -> Result<()> {
let client = self
.client
.upgrade()
.ok_or(Error::RCL_RET_ACTION_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
client.register_poll_available(sender);
Ok(())
}
}

792
src/action_servers.rs Normal file
View File

@ -0,0 +1,792 @@
use super::*;
use futures::future::{join_all, JoinAll};
#[derive(Clone)]
pub struct ActionServer<T>
where
T: WrappedActionTypeSupport,
{
server: Weak<Mutex<WrappedActionServer<T>>>,
}
impl<T> ActionServer<T>
where
T: WrappedActionTypeSupport,
{
pub fn is_valid(&self) -> Result<bool> {
// upgrade to actual ref. if still alive
let action_server = self
.server
.upgrade()
.ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
let action_server = action_server.lock().unwrap();
Ok(unsafe { rcl_action_server_is_valid(&action_server.rcl_handle) })
}
}
pub trait ActionServer_ {
fn handle(&self) -> &rcl_action_server_t;
fn handle_mut(&mut self) -> &mut rcl_action_server_t;
fn handle_goal_request(&mut self, server: Arc<Mutex<dyn ActionServer_>>) -> ();
fn send_completed_cancel_requests(&mut self) -> ();
fn handle_cancel_request(&mut self) -> ();
fn handle_result_request(&mut self) -> ();
fn handle_goal_expired(&mut self) -> ();
fn publish_status(&self) -> ();
fn set_goal_state(
&mut self,
uuid: &uuid::Uuid,
new_state: rcl_action_goal_event_t,
) -> Result<()>;
fn add_result(&mut self, uuid: uuid::Uuid, msg: Box<dyn VoidPtr>) -> ();
fn cancel_goal(&mut self, uuid: &uuid::Uuid);
fn is_cancelling(&self, uuid: &uuid::Uuid) -> Result<bool>;
fn add_goal_handle(
&mut self,
uuid: uuid::Uuid,
goal_handle: *mut rcl_action_goal_handle_t,
) -> ();
fn destroy(&mut self, node: &mut rcl_node_t);
}
pub struct CancelRequest {
pub uuid: uuid::Uuid,
response_sender: oneshot::Sender<(uuid::Uuid, bool)>,
}
impl CancelRequest {
/// Accepts the cancel request. The action server should now cancel the corresponding goal.
pub fn accept(self) {
match self.response_sender.send((self.uuid, true)) {
Err(_) => eprintln!("warning: could not send goal canellation accept msg"),
_ => (),
}
}
/// Rejects the cancel request.
pub fn reject(self) {
match self.response_sender.send((self.uuid, false)) {
Err(_) => eprintln!("warning: could not send goal cancellation rejection"),
_ => (),
}
}
}
pub struct GoalRequest<T>
where
T: WrappedActionTypeSupport,
{
pub uuid: uuid::Uuid,
pub goal: T::Goal,
cancel_requests: mpsc::Receiver<CancelRequest>,
server: Weak<Mutex<dyn ActionServer_>>,
request_id: rmw_request_id_t,
}
unsafe impl<T> Send for GoalRequest<T> where T: WrappedActionTypeSupport {}
impl<T: 'static> GoalRequest<T>
where
T: WrappedActionTypeSupport,
{
/// Accept the goal request and become a ServerGoal.
/// Returns a handle to the goal and a stream on which cancel requests can be received.
pub fn accept(mut self) -> Result<(ServerGoal<T>, impl Stream<Item = CancelRequest> + Unpin)> {
let uuid_msg = unique_identifier_msgs::msg::UUID {
uuid: self.uuid.as_bytes().to_vec(),
};
let time = builtin_interfaces::msg::Time::default();
let goal_info = action_msgs::msg::GoalInfo {
goal_id: uuid_msg,
stamp: time.clone(),
};
let native_goal_info = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::from(&goal_info);
let server = self.server.upgrade().unwrap(); // todo fixme
let mut server = server.lock().unwrap();
let goal_handle: *mut rcl_action_goal_handle_t =
unsafe { rcl_action_accept_new_goal(server.handle_mut(), &*native_goal_info) };
// send response
let response_msg = T::make_goal_response_msg(true, time);
let mut response_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
>::from(&response_msg);
let ret = unsafe {
rcl_action_send_goal_response(
server.handle_mut(),
&mut self.request_id,
response_msg.void_ptr_mut(),
)
};
if ret != RCL_RET_OK as i32 {
return Err(Error::from_rcl_error(ret));
}
unsafe {
rcl_action_update_goal_state(goal_handle, rcl_action_goal_event_t::GOAL_EVENT_EXECUTE);
}
server.publish_status();
let g = ServerGoal {
uuid: self.uuid.clone(),
goal: self.goal,
server: self.server,
};
// server.goals.insert(g.uuid.clone(), goal_handle);
server.add_goal_handle(g.uuid.clone(), goal_handle);
return Ok((g, self.cancel_requests));
}
/// reject the goal request and be consumed in the process
pub fn reject(mut self) -> Result<()> {
let time = builtin_interfaces::msg::Time::default();
let server = self.server.upgrade().unwrap(); // todo fixme
let mut server = server.lock().unwrap();
let response_msg = T::make_goal_response_msg(false, time);
let mut response_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
>::from(&response_msg);
let ret = unsafe {
rcl_action_send_goal_response(
server.handle_mut(),
&mut self.request_id,
response_msg.void_ptr_mut(),
)
};
if ret != RCL_RET_OK as i32 {
return Err(Error::from_rcl_error(ret));
}
Ok(())
}
}
pub struct WrappedActionServer<T>
where
T: WrappedActionTypeSupport,
{
pub rcl_handle: rcl_action_server_t,
pub clock_handle: Box<rcl_clock_t>,
pub goal_request_sender: mpsc::Sender<GoalRequest<T>>,
pub cancel_senders: HashMap<uuid::Uuid, mpsc::Sender<CancelRequest>>,
pub active_cancel_requests: Vec<(
rmw_request_id_t,
action_msgs::srv::CancelGoal::Response,
JoinAll<oneshot::Receiver<(uuid::Uuid, bool)>>,
)>,
pub goals: HashMap<uuid::Uuid, *mut rcl_action_goal_handle_t>,
pub result_msgs: HashMap<uuid::Uuid, Box<dyn VoidPtr>>,
pub result_requests: HashMap<uuid::Uuid, Vec<rmw_request_id_t>>,
}
impl<T: 'static> ActionServer_ for WrappedActionServer<T>
where
T: WrappedActionTypeSupport,
{
fn handle(&self) -> &rcl_action_server_t {
&self.rcl_handle
}
fn handle_mut(&mut self) -> &mut rcl_action_server_t {
&mut self.rcl_handle
}
fn is_cancelling(&self, uuid: &uuid::Uuid) -> Result<bool> {
if let Some(handle) = self.goals.get(uuid) {
let mut state = 0u8; // TODO: int8 STATUS_UNKNOWN = 0;
let ret = unsafe { rcl_action_goal_handle_get_status(*handle, &mut state) };
if ret != RCL_RET_OK as i32 {
println!("action server: Failed to get goal handle state: {}", ret);
return Err(Error::from_rcl_error(ret));
}
return Ok(state == 3u8); // TODO: int8 STATUS_CANCELING
}
Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID)
}
fn cancel_goal(&mut self, uuid: &uuid::Uuid) {
if let Some(handle) = self.goals.remove(uuid) {
let ret = unsafe {
rcl_action_update_goal_state(
handle,
rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL,
)
};
if ret != RCL_RET_OK as i32 {
println!(
"action server: could not cancel goal: {}",
Error::from_rcl_error(ret)
);
}
}
}
fn set_goal_state(
&mut self,
uuid: &uuid::Uuid,
new_state: rcl_action_goal_event_t,
) -> Result<()> {
let goal_info = action_msgs::msg::GoalInfo {
goal_id: unique_identifier_msgs::msg::UUID {
uuid: uuid.as_bytes().to_vec(),
},
..action_msgs::msg::GoalInfo::default()
};
let goal_info_native = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::from(&goal_info);
// does this goal exist?
let goal_exists =
unsafe { rcl_action_server_goal_exists(self.handle(), &*goal_info_native) };
if !goal_exists {
println!("tried to publish result without a goal");
return Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID);
}
if let Some(handle) = self.goals.get(uuid) {
// todo: error handling
unsafe {
rcl_action_update_goal_state(*handle, new_state);
}
// todo: error handling
unsafe {
rcl_action_notify_goal_done(self.handle());
}
// send out updated statues
self.publish_status();
Ok(())
} else {
return Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID);
}
}
fn send_completed_cancel_requests(&mut self) {
let mut canceled = vec![];
let mut responses = vec![];
self.active_cancel_requests
.retain_mut(|(request_id, msg, fut)| {
let boxed = fut.boxed();
if let Some(results) = boxed.now_or_never() {
let mut response_msg = msg.clone();
let requested_cancels = response_msg.goals_canceling.len();
for r in results {
match r {
Ok((uuid, do_cancel)) => {
// cancel goal and filter response msg.
if do_cancel {
canceled.push(uuid.clone());
}
response_msg.goals_canceling.retain(|goal_info| {
let msg_uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(
goal_info.goal_id.uuid.clone(),
));
do_cancel && msg_uuid == uuid
});
}
Err(oneshot::Canceled) => {
eprintln!("Warning, cancel request not handled!");
return false; // skip this request.
}
}
}
// check if all cancels were rejected.
if requested_cancels >= 1 && response_msg.goals_canceling.is_empty() {
response_msg.return_code = 1; // TODO: auto generate these (int8 ERROR_REJECTED=1)
}
responses.push((*request_id, response_msg));
false
} else {
true
}
});
canceled.iter().for_each(|uuid| self.cancel_goal(&uuid));
if !canceled.is_empty() {
// at least one goal state changed, publish a new status message
self.publish_status();
}
// send out responses
for (mut request_id, response_msg) in responses {
// send out response msg.
let mut native_msg =
WrappedNativeMsg::<action_msgs::srv::CancelGoal::Response>::from(&response_msg);
let ret = unsafe {
rcl_action_send_cancel_response(
&self.rcl_handle,
&mut request_id,
native_msg.void_ptr_mut(),
)
};
if ret != RCL_RET_OK as i32 {
println!("action server: could send cancel response. {}", ret);
}
}
}
fn handle_goal_request(&mut self, server: Arc<Mutex<dyn ActionServer_>>) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut request_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request,
>::new();
let ret = unsafe {
rcl_action_take_goal_request(
&self.rcl_handle,
request_id.as_mut_ptr(),
request_msg.void_ptr_mut(),
)
};
if ret != RCL_RET_OK as i32 {
// this seems normal if client dies.
return;
}
let msg = <<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request>::from_native(&request_msg);
let (uuid_msg, goal) = T::destructure_goal_request_msg(msg);
let uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(uuid_msg.uuid.clone()));
let (cancel_sender, cancel_receiver) = mpsc::channel::<CancelRequest>(10);
self.cancel_senders.insert(uuid.clone(), cancel_sender);
let gr: GoalRequest<T> = GoalRequest {
uuid,
goal,
cancel_requests: cancel_receiver,
server: Arc::downgrade(&server),
request_id: unsafe { request_id.assume_init() },
};
// send out request.
match self.goal_request_sender.try_send(gr) {
Err(e) => eprintln!("warning: could not send service request ({})", e),
_ => (),
}
}
fn handle_cancel_request(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut request_msg = WrappedNativeMsg::<action_msgs::srv::CancelGoal::Request>::new();
let ret = unsafe {
rcl_action_take_cancel_request(
&self.rcl_handle,
request_id.as_mut_ptr(),
request_msg.void_ptr_mut(),
)
};
let request_id = unsafe { request_id.assume_init() };
if ret != RCL_RET_OK as i32 {
// this seems normal if client dies.
return;
}
let mut cancel_response = unsafe { rcl_action_get_zero_initialized_cancel_response() };
let ret = unsafe {
rcl_action_process_cancel_request(&self.rcl_handle, &*request_msg, &mut cancel_response)
};
if ret != RCL_RET_OK as i32 {
println!("action server: could not process cancel request. {}", ret);
return;
}
let response_msg =
action_msgs::srv::CancelGoal::Response::from_native(&cancel_response.msg);
let return_channels = response_msg
.goals_canceling
.iter()
.flat_map(|goal_info| {
let uuid =
uuid::Uuid::from_bytes(vec_to_uuid_bytes(goal_info.goal_id.uuid.clone()));
self.cancel_senders
.get_mut(&uuid)
.and_then(|cancel_sender| {
let (s, r) = oneshot::channel::<(uuid::Uuid, bool)>();
let cr = CancelRequest {
uuid: uuid.clone(),
response_sender: s,
};
match cancel_sender.try_send(cr) {
Err(_) => {
eprintln!("warning: could not send goal cancellation request");
None
}
_ => Some(r),
}
})
})
.collect::<Vec<_>>();
// because we want to reply to the caller when all goals have been either accepted or rejected,
// join the channels into one future that we can poll during spin.
self.active_cancel_requests
.push((request_id, response_msg, join_all(return_channels)));
}
fn handle_goal_expired(&mut self) {
let mut goal_info = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::new();
let mut num_expired = 1;
while num_expired > 1 {
let ret = unsafe {
rcl_action_expire_goals(&self.rcl_handle, &mut *goal_info, 1, &mut num_expired)
};
if ret != RCL_RET_OK as i32 {
println!("action server: could not expire goal.");
return;
}
let gi = action_msgs::msg::GoalInfo::from_native(&goal_info);
let uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(gi.goal_id.uuid.clone()));
println!("goal expired: {} - {}", uuid, num_expired);
// todo
// self.goals.remove(&uuid);
self.result_msgs.remove(&uuid);
self.result_requests.remove(&uuid);
}
}
fn publish_status(&self) {
unsafe {
let mut status = rcl_action_get_zero_initialized_goal_status_array();
let ret = rcl_action_get_goal_status_array(&self.rcl_handle, &mut status);
if ret != RCL_RET_OK as i32 {
println!(
"action server: failed to get goal status array: {}",
Error::from_rcl_error(ret)
);
return;
}
let ret = rcl_action_publish_status(
&self.rcl_handle,
&status as *const _ as *const std::os::raw::c_void,
);
if ret != RCL_RET_OK as i32 {
println!(
"action server: failed to publish status: {}",
Error::from_rcl_error(ret)
);
return;
}
rcl_action_goal_status_array_fini(&mut status);
}
}
fn add_goal_handle(
&mut self,
uuid: uuid::Uuid,
goal_handle: *mut rcl_action_goal_handle_t,
) -> () {
self.goals.insert(uuid, goal_handle);
}
// bit of a hack...
fn add_result(&mut self, uuid: uuid::Uuid, mut msg: Box<dyn VoidPtr>) -> () {
// if there are already requests for this goal, send the result immediately.
if let Some(rr) = self.result_requests.remove(&uuid) {
for mut req in rr {
let ret = unsafe {
rcl_action_send_result_response(&self.rcl_handle, &mut req, msg.void_ptr_mut())
};
if ret != RCL_RET_OK as i32 {
println!(
"action server: could send result request response. {}",
Error::from_rcl_error(ret)
);
}
}
}
self.result_msgs.insert(uuid, msg);
}
fn handle_result_request(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut request_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request,
>::new();
let ret = unsafe {
rcl_action_take_result_request(
&self.rcl_handle,
request_id.as_mut_ptr(),
request_msg.void_ptr_mut(),
)
};
if ret != RCL_RET_OK as i32 {
// this seems normal if client dies.
return;
}
let msg = <<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request>::from_native(&request_msg);
let goal_info = action_msgs::msg::GoalInfo {
goal_id: T::destructure_result_request_msg(msg),
..action_msgs::msg::GoalInfo::default()
};
let goal_info_native = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::from(&goal_info);
// does this goal exist?
let goal_exists =
unsafe { rcl_action_server_goal_exists(&self.rcl_handle, &*goal_info_native) };
let uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(goal_info.goal_id.uuid.clone()));
let response_msg = if !goal_exists {
// Goal does not exists
println!("goal does not exist :(");
let status = GoalStatus::Unknown;
let msg = T::make_result_response_msg(status.to_rcl(), T::Result::default());
let mut response_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
>::from(&msg);
Some(response_msg.void_ptr_mut())
} else {
self.result_msgs
.get_mut(&uuid)
.map(|msg| msg.void_ptr_mut())
};
let mut request_id = unsafe { request_id.assume_init() };
if let Some(response_msg) = response_msg {
let ret = unsafe {
rcl_action_send_result_response(&self.rcl_handle, &mut request_id, response_msg)
};
if ret != RCL_RET_OK as i32 {
println!(
"action server: could send result request response. {}",
Error::from_rcl_error(ret)
);
return;
}
} else {
// keep request for later when result comes in
// todo: add logic that replies to the requests
self.result_requests
.entry(uuid)
.or_insert(vec![])
.push(request_id);
}
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_action_server_fini(&mut self.rcl_handle, node);
rcl_ros_clock_fini(self.clock_handle.as_mut());
}
}
}
#[derive(Clone)]
pub struct ServerGoal<T>
where
T: WrappedActionTypeSupport,
{
pub uuid: uuid::Uuid,
pub goal: T::Goal,
server: Weak<Mutex<dyn ActionServer_>>,
}
unsafe impl<T> Send for ServerGoal<T> where T: WrappedActionTypeSupport {}
impl<T: 'static> ServerGoal<T>
where
T: WrappedActionTypeSupport,
{
pub fn is_cancelling(&self) -> Result<bool> {
let action_server = self
.server
.upgrade()
.ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
let action_server = action_server.lock().unwrap();
action_server.is_cancelling(&self.uuid)
}
pub fn publish_feedback(&self, msg: T::Feedback) -> Result<()>
where
T: WrappedActionTypeSupport,
{
// upgrade to actual ref. if still alive
let action_server = self
.server
.upgrade()
.ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
let uuid_msg = unique_identifier_msgs::msg::UUID {
uuid: self.uuid.as_bytes().to_vec(),
};
let feedback_msg = T::make_feedback_msg(uuid_msg, msg);
let mut native_msg = WrappedNativeMsg::<T::FeedbackMessage>::from(&feedback_msg);
let ret = unsafe {
rcl_action_publish_feedback(
action_server.lock().unwrap().handle(),
native_msg.void_ptr_mut(),
)
};
if ret != RCL_RET_OK as i32 {
eprintln!("coult not publish {}", Error::from_rcl_error(ret));
}
Ok(()) // todo: error codes
}
pub fn cancel(&mut self, msg: T::Result) -> Result<()> {
// upgrade to actual ref. if still alive
let action_server = self
.server
.upgrade()
.ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
let mut action_server = action_server.lock().unwrap();
// todo: check that the goal exists
let goal_info = action_msgs::msg::GoalInfo {
goal_id: unique_identifier_msgs::msg::UUID {
uuid: self.uuid.as_bytes().to_vec(),
},
..action_msgs::msg::GoalInfo::default()
};
let goal_info_native = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::from(&goal_info);
// does this goal exist?
let goal_exists =
unsafe { rcl_action_server_goal_exists(action_server.handle(), &*goal_info_native) };
if !goal_exists {
println!("tried to publish result without a goal");
return Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID);
}
// todo: error handling
unsafe {
rcl_action_notify_goal_done(action_server.handle());
}
// send out updated statues
action_server.publish_status();
// create result message
let result_msg = T::make_result_response_msg(5, msg); // todo: int8 STATUS_CANCELED = 5
let native_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
>::from(&result_msg);
action_server.add_result(self.uuid.clone(), Box::new(native_msg));
Ok(())
}
pub fn abort(&mut self, msg: T::Result) -> Result<()> {
// upgrade to actual ref. if still alive
let action_server = self
.server
.upgrade()
.ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
let mut action_server = action_server.lock().unwrap();
action_server.set_goal_state(&self.uuid, rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
// create result message
let result_msg = T::make_result_response_msg(6, msg); // todo: int8 STATUS_ABORTED = 6
let native_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
>::from(&result_msg);
action_server.add_result(self.uuid.clone(), Box::new(native_msg));
Ok(())
}
pub fn succeed(&mut self, msg: T::Result) -> Result<()>
where
T: WrappedActionTypeSupport,
{
// upgrade to actual ref. if still alive
let action_server = self
.server
.upgrade()
.ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
let mut action_server = action_server.lock().unwrap();
action_server.set_goal_state(&self.uuid, rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
// create result message
let result_msg = T::make_result_response_msg(4, msg); // todo: int8 STATUS_SUCCEEDED = 4
let native_msg = WrappedNativeMsg::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
>::from(&result_msg);
action_server.add_result(self.uuid.clone(), Box::new(native_msg));
Ok(())
}
}
pub fn create_action_server_helper(
node: &mut rcl_node_t,
action_name: &str,
clock_handle: *mut rcl_clock_t,
action_ts: *const rosidl_action_type_support_t,
) -> Result<rcl_action_server_t> {
let mut server_handle = unsafe { rcl_action_get_zero_initialized_server() };
let action_name_c_string =
CString::new(action_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
let result = unsafe {
let server_options = rcl_action_server_get_default_options();
rcl_action_server_init(
&mut server_handle,
node,
clock_handle,
action_ts,
action_name_c_string.as_ptr(),
&server_options,
)
};
if result == RCL_RET_OK as i32 {
Ok(server_handle)
} else {
Err(Error::from_rcl_error(result))
}
}
pub fn action_server_get_num_waits(
rcl_handle: &rcl_action_server_t,
num_subs: &mut usize,
num_gc: &mut usize,
num_timers: &mut usize,
num_clients: &mut usize,
num_services: &mut usize,
) -> Result<()> {
unsafe {
let result = rcl_action_server_wait_set_get_num_entities(
rcl_handle,
num_subs,
num_gc,
num_timers,
num_clients,
num_services,
);
if result == RCL_RET_OK as i32 {
Ok(())
} else {
Err(Error::from_rcl_error(result))
}
}
}

352
src/clients.rs Normal file
View File

@ -0,0 +1,352 @@
use super::*;
// Public facing apis
pub struct Client<T>
where
T: WrappedServiceTypeSupport,
{
client: Weak<Mutex<TypedClient<T>>>,
}
impl<T: 'static> Client<T>
where
T: WrappedServiceTypeSupport,
{
pub fn request(&self, msg: &T::Request) -> Result<impl Future<Output = Result<T::Response>>>
where
T: WrappedServiceTypeSupport,
{
// upgrade to actual ref. if still alive
let client = self.client.upgrade().ok_or(Error::RCL_RET_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
client.request(msg)
}
}
pub struct UntypedClient {
client: Weak<Mutex<UntypedClient_>>,
}
impl UntypedClient {
pub fn request(
&self,
msg: serde_json::Value,
) -> Result<impl Future<Output = Result<Result<serde_json::Value>>>> {
// upgrade to actual ref. if still alive
let client = self.client.upgrade().ok_or(Error::RCL_RET_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
client.request(msg)
}
}
pub fn make_client<T>(client: Weak<Mutex<TypedClient<T>>>) -> Client<T>
where
T: WrappedServiceTypeSupport,
{
Client { client }
}
pub fn make_untyped_client(client: Weak<Mutex<UntypedClient_>>) -> UntypedClient {
UntypedClient { client }
}
unsafe impl<T> Send for TypedClient<T> where T: WrappedServiceTypeSupport {}
impl<T: 'static> TypedClient<T>
where
T: WrappedServiceTypeSupport,
{
pub fn request(&mut self, msg: &T::Request) -> Result<impl Future<Output = Result<T::Response>>>
where
T: WrappedServiceTypeSupport,
{
let native_msg: WrappedNativeMsg<T::Request> = WrappedNativeMsg::<T::Request>::from(msg);
let mut seq_no = 0i64;
let result =
unsafe { rcl_send_request(&self.rcl_handle, native_msg.void_ptr(), &mut seq_no) };
let (sender, receiver) = oneshot::channel::<T::Response>();
if result == RCL_RET_OK as i32 {
self.response_channels.push((seq_no, sender));
// instead of "canceled" we return invalid client.
Ok(receiver.map_err(|_| Error::RCL_RET_CLIENT_INVALID))
} else {
eprintln!("coult not send request {}", result);
Err(Error::from_rcl_error(result))
}
}
}
unsafe impl Send for UntypedClient_ {}
impl UntypedClient_ {
pub fn request(
&mut self,
msg: serde_json::Value,
) -> Result<impl Future<Output = Result<Result<serde_json::Value>>>> {
let mut native_msg = (self.service_type.make_request_msg)();
native_msg.from_json(msg)?;
let mut seq_no = 0i64;
let result =
unsafe { rcl_send_request(&self.rcl_handle, native_msg.void_ptr(), &mut seq_no) };
let (sender, receiver) = oneshot::channel::<Result<serde_json::Value>>();
if result == RCL_RET_OK as i32 {
self.response_channels.push((seq_no, sender));
// instead of "canceled" we return invalid client.
Ok(receiver.map_err(|_| Error::RCL_RET_CLIENT_INVALID))
} else {
eprintln!("coult not send request {}", result);
Err(Error::from_rcl_error(result))
}
}
}
pub trait Client_ {
fn handle(&self) -> &rcl_client_t;
fn handle_response(&mut self) -> ();
fn register_poll_available(&mut self, s: oneshot::Sender<()>) -> ();
fn poll_available(&mut self, node: &mut rcl_node_t) -> ();
fn destroy(&mut self, node: &mut rcl_node_t) -> ();
}
pub struct TypedClient<T>
where
T: WrappedServiceTypeSupport,
{
pub rcl_handle: rcl_client_t,
pub response_channels: Vec<(i64, oneshot::Sender<T::Response>)>,
pub poll_available_channels: Vec<oneshot::Sender<()>>,
}
impl<T: 'static> Client_ for TypedClient<T>
where
T: WrappedServiceTypeSupport,
{
fn handle(&self) -> &rcl_client_t {
&self.rcl_handle
}
fn handle_response(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut response_msg = WrappedNativeMsg::<T::Response>::new();
let ret = unsafe {
rcl_take_response(
&self.rcl_handle,
request_id.as_mut_ptr(),
response_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
if let Some(idx) = self
.response_channels
.iter()
.position(|(id, _)| id == &request_id.sequence_number)
{
let (_, sender) = self.response_channels.swap_remove(idx);
let response = T::Response::from_native(&response_msg);
match sender.send(response) {
Ok(()) => {}
Err(e) => {
println!("error sending to client: {:?}", e);
}
}
} else {
let we_have: String = self
.response_channels
.iter()
.map(|(id, _)| id.to_string())
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
request_id.sequence_number, we_have
);
}
} // TODO handle failure.
}
fn register_poll_available(&mut self, s: oneshot::Sender<()>) {
self.poll_available_channels.push(s);
}
fn poll_available(&mut self, node: &mut rcl_node_t) {
if self.poll_available_channels.is_empty() {
return;
}
let available = service_available_helper(node, self.handle());
match available {
Ok(true) => {
// send ok and close channels
while let Some(sender) = self.poll_available_channels.pop() {
let _res = sender.send(()); // we ignore if receiver dropped.
}
}
Ok(false) => {
// not available...
}
Err(_) => {
// error, close all channels
self.poll_available_channels.clear();
}
}
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_client_fini(&mut self.rcl_handle, node);
}
}
}
pub struct UntypedClient_ {
pub service_type: UntypedServiceSupport,
pub rcl_handle: rcl_client_t,
pub response_channels: Vec<(i64, oneshot::Sender<Result<serde_json::Value>>)>,
pub poll_available_channels: Vec<oneshot::Sender<()>>,
}
impl Client_ for UntypedClient_ {
fn handle(&self) -> &rcl_client_t {
&self.rcl_handle
}
fn handle_response(&mut self) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut response_msg = (self.service_type.make_response_msg)();
let ret = unsafe {
rcl_take_response(
&self.rcl_handle,
request_id.as_mut_ptr(),
response_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
if let Some(idx) = self
.response_channels
.iter()
.position(|(id, _)| id == &request_id.sequence_number)
{
let (_, sender) = self.response_channels.swap_remove(idx);
let response = response_msg.to_json();
match sender.send(response) {
Ok(()) => {}
Err(e) => {
println!("error sending to client: {:?}", e);
}
}
} else {
let we_have: String = self
.response_channels
.iter()
.map(|(id, _)| id.to_string())
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
request_id.sequence_number, we_have
);
}
} // TODO handle failure.
}
fn register_poll_available(&mut self, s: oneshot::Sender<()>) {
self.poll_available_channels.push(s);
}
fn poll_available(&mut self, node: &mut rcl_node_t) {
if self.poll_available_channels.is_empty() {
return;
}
let available = service_available_helper(node, self.handle());
match available {
Ok(true) => {
// send ok and close channels
while let Some(sender) = self.poll_available_channels.pop() {
let _res = sender.send(()); // we ignore if receiver dropped.
}
}
Ok(false) => {
// not available...
}
Err(_) => {
// error, close all channels
self.poll_available_channels.clear();
}
}
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_client_fini(&mut self.rcl_handle, node);
}
}
}
pub fn create_client_helper(
node: *mut rcl_node_t,
service_name: &str,
service_ts: *const rosidl_service_type_support_t,
) -> Result<rcl_client_t> {
let mut client_handle = unsafe { rcl_get_zero_initialized_client() };
let service_name_c_string =
CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
let result = unsafe {
let client_options = rcl_client_get_default_options();
rcl_client_init(
&mut client_handle,
node,
service_ts,
service_name_c_string.as_ptr(),
&client_options,
)
};
if result == RCL_RET_OK as i32 {
Ok(client_handle)
} else {
Err(Error::from_rcl_error(result))
}
}
pub fn service_available_helper(node: &mut rcl_node_t, client: &rcl_client_t) -> Result<bool> {
let mut avail = false;
let result = unsafe { rcl_service_server_is_available(node, client, &mut avail) };
if result == RCL_RET_OK as i32 {
Ok(avail)
} else {
Err(Error::from_rcl_error(result))
}
}
use crate::nodes::IsAvailablePollable;
impl<T: 'static> IsAvailablePollable for Client<T>
where
T: WrappedServiceTypeSupport,
{
fn register_poll_available(&self, sender: oneshot::Sender<()>) -> Result<()> {
let client = self.client.upgrade().ok_or(Error::RCL_RET_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
client.register_poll_available(sender);
Ok(())
}
}
impl IsAvailablePollable for UntypedClient {
fn register_poll_available(&self, sender: oneshot::Sender<()>) -> Result<()> {
let client = self.client.upgrade().ok_or(Error::RCL_RET_CLIENT_INVALID)?;
let mut client = client.lock().unwrap();
client.register_poll_available(sender);
Ok(())
}
}

76
src/clocks.rs Normal file
View File

@ -0,0 +1,76 @@
use super::*;
#[derive(Debug)]
pub enum ClockType {
RosTime,
SystemTime,
SteadyTime,
}
unsafe impl Send for Clock {}
pub struct Clock {
clock_handle: Box<rcl_clock_t>,
}
pub fn clock_type_to_rcl(ct: &ClockType) -> rcl_clock_type_t {
match ct {
ClockType::RosTime => rcl_clock_type_t::RCL_ROS_TIME,
ClockType::SystemTime => rcl_clock_type_t::RCL_SYSTEM_TIME,
ClockType::SteadyTime => rcl_clock_type_t::RCL_STEADY_TIME,
}
}
impl Clock {
pub fn create(ct: ClockType) -> Result<Clock> {
let mut clock_handle = MaybeUninit::<rcl_clock_t>::uninit();
let rcl_ct = clock_type_to_rcl(&ct);
let ret = unsafe {
rcl_clock_init(
rcl_ct,
clock_handle.as_mut_ptr(),
&mut rcutils_get_default_allocator(),
)
};
if ret != RCL_RET_OK as i32 {
eprintln!("could not create {:?} clock: {}", ct, ret);
return Err(Error::from_rcl_error(ret));
}
let clock_handle = Box::new(unsafe { clock_handle.assume_init() });
Ok(Clock { clock_handle })
}
pub fn get_now(&mut self) -> Result<Duration> {
let valid = unsafe { rcl_clock_valid(&mut *self.clock_handle) };
if !valid {
return Err(Error::from_rcl_error(RCL_RET_INVALID_ARGUMENT as i32));
}
let mut tp: rcutils_time_point_value_t = 0;
let ret = unsafe { rcl_clock_get_now(&mut *self.clock_handle, &mut tp) };
if ret != RCL_RET_OK as i32 {
eprintln!("could not create steady clock: {}", ret);
return Err(Error::from_rcl_error(ret));
}
let dur = Duration::from_nanos(tp as u64);
Ok(dur)
}
pub fn to_builtin_time(d: &Duration) -> builtin_interfaces::msg::Time {
let sec = d.as_secs() as i32;
let nanosec = d.subsec_nanos();
builtin_interfaces::msg::Time { sec, nanosec }
}
}
impl Drop for Clock {
fn drop(&mut self) {
unsafe {
rcl_clock_fini(&mut *self.clock_handle);
}
}
}

103
src/context.rs Normal file
View File

@ -0,0 +1,103 @@
use super::*;
#[derive(Debug, Clone)]
pub struct Context {
pub context_handle: Arc<Mutex<ContextHandle>>,
}
unsafe impl Send for Context {}
impl Context {
pub fn create() -> Result<Context> {
let mut ctx: Box<rcl_context_t> = unsafe { Box::new(rcl_get_zero_initialized_context()) };
// argc/v
let args = std::env::args()
.map(|arg| CString::new(arg).unwrap())
.collect::<Vec<CString>>();
let mut c_args = args
.iter()
.map(|arg| arg.as_ptr())
.collect::<Vec<*const ::std::os::raw::c_char>>();
c_args.push(std::ptr::null());
let is_valid = unsafe {
let allocator = rcutils_get_default_allocator();
let mut init_options = rcl_get_zero_initialized_init_options();
rcl_init_options_init(&mut init_options, allocator);
rcl_init(
(c_args.len() - 1) as ::std::os::raw::c_int,
c_args.as_ptr(),
&init_options,
ctx.as_mut(),
);
rcl_init_options_fini(&mut init_options as *mut _);
rcl_context_is_valid(ctx.as_mut())
};
let logging_ok = unsafe {
let _guard = log_guard();
let ret = rcl_logging_configure(
&ctx.as_ref().global_arguments,
&rcutils_get_default_allocator(),
);
ret == RCL_RET_OK as i32
};
if is_valid && logging_ok {
Ok(Context {
context_handle: Arc::new(Mutex::new(ContextHandle(ctx))),
})
} else {
Err(Error::RCL_RET_ERROR) // TODO
}
}
pub fn is_valid(&self) -> bool {
let mut ctx = self.context_handle.lock().unwrap();
unsafe { rcl_context_is_valid(ctx.as_mut()) }
}
}
#[derive(Debug)]
pub struct ContextHandle(Box<rcl_context_t>);
impl Deref for ContextHandle {
type Target = Box<rcl_context_t>;
fn deref<'a>(&'a self) -> &'a Box<rcl_context_t> {
&self.0
}
}
impl DerefMut for ContextHandle {
fn deref_mut<'a>(&'a mut self) -> &'a mut Box<rcl_context_t> {
&mut self.0
}
}
impl Drop for ContextHandle {
fn drop(&mut self) {
// TODO: error handling? atleast probably need rcl_reset_error
unsafe {
rcl::rcl_shutdown(self.0.as_mut());
rcl::rcl_context_fini(self.0.as_mut());
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_context_drop() -> () {
{
let ctx = Context::create().unwrap();
assert!(ctx.is_valid());
}
{
let ctx = Context::create().unwrap();
assert!(ctx.is_valid());
}
}
}

2
src/error.rs
View File

@ -2,7 +2,7 @@
use rcl::*;
use thiserror::Error;
// TODO
pub type Result<T> = std::result::Result<T, Error>;
#[derive(Error, Debug)]
pub enum Error {

3389
src/lib.rs
View File

File diff suppressed because it is too large Load Diff

682
src/msg_types.rs Normal file
View File

@ -0,0 +1,682 @@
use crate::error::*;
use msg_gen::*;
use rcl::{
rosidl_action_type_support_t, rosidl_message_type_support_t, rosidl_service_type_support_t,
};
use serde::{Deserialize, Serialize};
use std::fmt::Debug;
use std::ops::{Deref, DerefMut};
pub mod generated_msgs {
use super::*;
include!(concat!(env!("OUT_DIR"), "/_r2r_generated_msgs.rs"));
include!(concat!(
env!("OUT_DIR"),
"/_r2r_generated_untyped_helper.rs"
));
include!(concat!(
env!("OUT_DIR"),
"/_r2r_generated_service_helper.rs"
));
include!(concat!(env!("OUT_DIR"), "/_r2r_generated_action_helper.rs"));
}
use generated_msgs::{builtin_interfaces, unique_identifier_msgs};
pub trait WrappedTypesupport:
Serialize + serde::de::DeserializeOwned + Default + Debug + Clone
{
type CStruct;
fn get_ts() -> &'static rosidl_message_type_support_t;
fn create_msg() -> *mut Self::CStruct;
fn destroy_msg(msg: *mut Self::CStruct);
fn from_native(msg: &Self::CStruct) -> Self;
fn copy_to_native(&self, msg: &mut Self::CStruct);
}
pub trait WrappedServiceTypeSupport: Debug + Clone {
type Request: WrappedTypesupport;
type Response: WrappedTypesupport;
fn get_ts() -> &'static rosidl_service_type_support_t;
}
pub trait WrappedActionTypeSupport: Debug + Clone {
type Goal: WrappedTypesupport;
type Result: WrappedTypesupport;
type Feedback: WrappedTypesupport;
// internal...
type FeedbackMessage: WrappedTypesupport;
type SendGoal: WrappedServiceTypeSupport;
type GetResult: WrappedServiceTypeSupport;
fn get_ts() -> &'static rosidl_action_type_support_t;
fn make_goal_request_msg(
goal_id: unique_identifier_msgs::msg::UUID,
goal: Self::Goal,
) -> <<Self as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request;
fn make_goal_response_msg(
accepted: bool,
stamp: builtin_interfaces::msg::Time,
) -> <<Self as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response;
fn make_feedback_msg(
goal_id: unique_identifier_msgs::msg::UUID,
feedback: Self::Feedback,
) -> Self::FeedbackMessage;
fn make_result_request_msg(
goal_id: unique_identifier_msgs::msg::UUID,
) -> <<Self as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request;
fn make_result_response_msg(
status: i8,
result: Self::Result,
) -> <<Self as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response;
fn destructure_goal_request_msg(
msg: <<Self as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request,
) -> (unique_identifier_msgs::msg::UUID, Self::Goal);
fn destructure_goal_response_msg(
msg: <<Self as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
) -> (bool, builtin_interfaces::msg::Time);
fn destructure_feedback_msg(
msg: Self::FeedbackMessage,
) -> (unique_identifier_msgs::msg::UUID, Self::Feedback);
fn destructure_result_response_msg(
msg: <<Self as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
) -> (i8, Self::Result);
fn destructure_result_request_msg(
msg: <<Self as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request,
) -> unique_identifier_msgs::msg::UUID;
}
#[derive(Debug)]
pub struct WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
pub msg: *mut T::CStruct,
}
pub trait VoidPtr {
fn void_ptr(&self) -> *const std::os::raw::c_void;
fn void_ptr_mut(&mut self) -> *mut std::os::raw::c_void;
}
#[derive(Debug)]
pub struct WrappedNativeMsgUntyped {
pub ts: &'static rosidl_message_type_support_t,
msg: *mut std::os::raw::c_void,
destroy: fn(*mut std::os::raw::c_void),
msg_to_json: fn(
native: *const std::os::raw::c_void,
) -> std::result::Result<serde_json::Value, serde_json::error::Error>,
msg_from_json: fn(
native: *mut std::os::raw::c_void,
json: serde_json::Value,
) -> std::result::Result<(), serde_json::error::Error>,
}
unsafe impl Send for UntypedServiceSupport {}
pub struct UntypedServiceSupport {
pub ts: &'static rosidl_service_type_support_t,
pub make_request_msg: fn() -> WrappedNativeMsgUntyped,
pub make_response_msg: fn() -> WrappedNativeMsgUntyped,
}
impl UntypedServiceSupport {
fn new<T>() -> Self
where
T: WrappedServiceTypeSupport,
{
let make_request_msg = || WrappedNativeMsgUntyped::new::<T::Request>();
let make_response_msg = || WrappedNativeMsgUntyped::new::<T::Response>();
UntypedServiceSupport {
ts: T::get_ts(),
make_request_msg,
make_response_msg,
}
}
}
// For now only the client side is implemented.
unsafe impl Send for UntypedActionSupport {}
pub struct UntypedActionSupport {
pub(crate) ts: &'static rosidl_action_type_support_t,
pub(crate) make_goal_request_msg: Box<
dyn Fn(unique_identifier_msgs::msg::UUID, serde_json::Value) -> WrappedNativeMsgUntyped,
>,
pub(crate) make_goal_response_msg: Box<dyn Fn() -> WrappedNativeMsgUntyped>,
pub(crate) destructure_goal_response_msg:
Box<dyn Fn(WrappedNativeMsgUntyped) -> (bool, builtin_interfaces::msg::Time)>,
pub(crate) make_feedback_msg: Box<dyn Fn() -> WrappedNativeMsgUntyped>,
pub(crate) destructure_feedback_msg: Box<
dyn Fn(
WrappedNativeMsgUntyped,
) -> (unique_identifier_msgs::msg::UUID, Result<serde_json::Value>),
>,
pub(crate) make_result_request_msg:
Box<dyn Fn(unique_identifier_msgs::msg::UUID) -> WrappedNativeMsgUntyped>,
pub(crate) make_result_response_msg: Box<dyn Fn() -> WrappedNativeMsgUntyped>,
pub(crate) destructure_result_response_msg:
Box<dyn Fn(WrappedNativeMsgUntyped) -> (i8, Result<serde_json::Value>)>,
}
impl UntypedActionSupport {
fn new<T>() -> Self
where
T: WrappedActionTypeSupport,
{
// TODO: this is terrible. These closures perform json (de)serialization just to move the data.
// FIX.
let make_goal_request_msg = Box::new(|goal_id, goal| {
let goal_msg: T::Goal =
serde_json::from_value(goal).expect("TODO: move this error handling");
let request_msg = T::make_goal_request_msg(goal_id, goal_msg);
let json =
serde_json::to_value(request_msg.clone()).expect("TODO: move this error handling");
let mut native_untyped = WrappedNativeMsgUntyped::new::<
<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request,
>();
native_untyped
.from_json(json)
.expect("TODO: move this error handling");
native_untyped
});
let make_goal_response_msg = Box::new(|| {
WrappedNativeMsgUntyped::new::<
<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
>()
});
let destructure_goal_response_msg = Box::new(|msg: WrappedNativeMsgUntyped| {
let msg = unsafe {
<<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response>
::from_native(&*(msg.msg as *const <<<T as WrappedActionTypeSupport>::SendGoal as
WrappedServiceTypeSupport>::Response as WrappedTypesupport>::CStruct))
};
T::destructure_goal_response_msg(msg)
});
let make_feedback_msg = Box::new(|| WrappedNativeMsgUntyped::new::<T::FeedbackMessage>());
let destructure_feedback_msg = Box::new(|msg: WrappedNativeMsgUntyped| {
let msg = unsafe {
T::FeedbackMessage::from_native(
&*(msg.msg as *const <T::FeedbackMessage as WrappedTypesupport>::CStruct),
)
};
let (uuid, feedback) = T::destructure_feedback_msg(msg);
let json = serde_json::to_value(feedback).map_err(|serde_err| Error::SerdeError {
err: serde_err.to_string(),
});
(uuid, json)
});
let make_result_request_msg = Box::new(|uuid_msg: unique_identifier_msgs::msg::UUID| {
let request_msg = T::make_result_request_msg(uuid_msg);
let json =
serde_json::to_value(request_msg.clone()).expect("TODO: move this error handling");
let mut native_untyped = WrappedNativeMsgUntyped::new::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request,
>();
native_untyped
.from_json(json)
.expect("TODO: move this error handling");
native_untyped
});
let make_result_response_msg = Box::new(|| {
WrappedNativeMsgUntyped::new::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
>()
});
let destructure_result_response_msg = Box::new(|msg: WrappedNativeMsgUntyped| {
let msg = unsafe {
<<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response>
::from_native(&*(msg.msg as *const <<<T as WrappedActionTypeSupport>::GetResult as
WrappedServiceTypeSupport>::Response as WrappedTypesupport>::CStruct))
};
let (status, result) = T::destructure_result_response_msg(msg);
let json = serde_json::to_value(result).map_err(|serde_err| Error::SerdeError {
err: serde_err.to_string(),
});
(status, json)
});
UntypedActionSupport {
ts: T::get_ts(),
make_goal_request_msg,
make_goal_response_msg,
destructure_goal_response_msg,
make_feedback_msg,
destructure_feedback_msg,
make_result_request_msg,
make_result_response_msg,
destructure_result_response_msg,
// destructure_goal_response_msg,
// make_request_msg,
// make_response_msg,
}
}
}
impl WrappedNativeMsgUntyped {
fn new<T>() -> Self
where
T: WrappedTypesupport,
{
let destroy = |native: *mut std::os::raw::c_void| {
let native_msg = native as *mut T::CStruct;
T::destroy_msg(native_msg);
};
let msg_to_json = |native: *const std::os::raw::c_void| {
let msg = unsafe { T::from_native(&*(native as *const T::CStruct)) };
serde_json::to_value(&msg)
};
let msg_from_json = |native: *mut std::os::raw::c_void, json: serde_json::Value| {
serde_json::from_value(json).map(|msg: T| unsafe {
msg.copy_to_native(&mut *(native as *mut T::CStruct));
})
};
WrappedNativeMsgUntyped {
ts: T::get_ts(),
msg: T::create_msg() as *mut std::os::raw::c_void,
destroy,
msg_to_json,
msg_from_json,
}
}
pub fn to_json(&self) -> Result<serde_json::Value> {
let json = (self.msg_to_json)(self.msg);
json.map_err(|serde_err| Error::SerdeError {
err: serde_err.to_string(),
})
}
pub fn from_json(&mut self, json: serde_json::Value) -> Result<()> {
(self.msg_from_json)(self.msg, json).map_err(|serde_err| Error::SerdeError {
err: serde_err.to_string(),
})
}
}
impl VoidPtr for WrappedNativeMsgUntyped {
fn void_ptr(&self) -> *const std::os::raw::c_void {
self.msg as *const _ as *const std::os::raw::c_void
}
fn void_ptr_mut(&mut self) -> *mut std::os::raw::c_void {
self.msg as *mut _ as *mut std::os::raw::c_void
}
}
impl Drop for WrappedNativeMsgUntyped {
fn drop(&mut self) {
(self.destroy)(self.msg);
}
}
impl<T> WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
pub fn new() -> Self {
WrappedNativeMsg {
msg: T::create_msg(),
}
}
pub fn from(msg: &T) -> Self {
let mut native_msg = Self::new();
msg.copy_to_native(&mut native_msg);
native_msg
}
}
impl<T: 'static> VoidPtr for WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
fn void_ptr(&self) -> *const std::os::raw::c_void {
self.msg as *const _ as *const std::os::raw::c_void
}
fn void_ptr_mut(&mut self) -> *mut std::os::raw::c_void {
self.msg as *mut _ as *mut std::os::raw::c_void
}
}
impl<T> Drop for WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
fn drop(&mut self) {
T::destroy_msg(self.msg);
}
}
impl<T> Deref for WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
type Target = T::CStruct;
fn deref(&self) -> &Self::Target {
unsafe { &(*self.msg) }
}
}
impl<T> DerefMut for WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
fn deref_mut(&mut self) -> &mut Self::Target {
unsafe { &mut (*self.msg) }
}
}
#[cfg(test)]
mod tests {
use super::generated_msgs::*;
use super::*;
use rcl::*;
#[test]
fn test_ros_str() -> () {
let hej = "hej hopp";
let mut msg = WrappedNativeMsg::<std_msgs::msg::String>::new();
msg.data.assign(hej);
assert_eq!(msg.data.to_str(), hej);
}
#[test]
fn test_copy_fields() -> () {
let msg_orig = std_msgs::msg::String { data: "hej".into() };
let rosmsg = WrappedNativeMsg::<std_msgs::msg::String>::from(&msg_orig);
let msg2 = std_msgs::msg::String::from_native(&rosmsg);
assert_eq!(msg_orig, msg2);
}
#[test]
fn test_introspection_string() -> () {
unsafe {
use std::ffi::CStr;
let x = rosidl_typesupport_introspection_c__get_message_type_support_handle__std_msgs__msg__String();
let members = (*x).data as *const rosidl_typesupport_introspection_c__MessageMembers;
println!("{:#?}", *members);
assert_eq!((*members).member_count_, 1);
let s = CStr::from_ptr((*members).message_namespace_)
.to_str()
.unwrap();
assert_eq!(s, "std_msgs__msg");
let s = CStr::from_ptr((*members).message_name_).to_str().unwrap();
assert_eq!(s, "String");
let member = (*members).members_;
println!("member: {:#?}", *member);
let field_name = CStr::from_ptr((*member).name_).to_str().unwrap();
let type_id = (*member).type_id_;
let is_array = (*member).is_array_;
assert_eq!(field_name, "data");
assert_eq!(
type_id,
rosidl_typesupport_introspection_c__ROS_TYPE_STRING as u8
);
assert_eq!(is_array, false);
}
}
#[test]
#[should_panic] // we are testing that we cannot have to many elements in a fixed sized field
fn test_fixedsizearray() -> () {
unsafe {
let x = rosidl_typesupport_introspection_c__get_message_type_support_handle__geometry_msgs__msg__AccelWithCovariance();
let members = (*x).data as *const rosidl_typesupport_introspection_c__MessageMembers;
println!("{:#?}", *members);
let memberslice =
std::slice::from_raw_parts((*members).members_, (*members).member_count_ as usize);
for member in memberslice {
println!("member: {:#?}", *member);
}
let msg_native = WrappedNativeMsg::<geometry_msgs::msg::AccelWithCovariance>::new();
let mut msg = geometry_msgs::msg::AccelWithCovariance::from_native(&msg_native);
println!("{:#?}", msg);
msg.covariance[0] = 10.0;
msg.covariance[10] = 10.0;
msg.covariance[35] = 99.0;
msg.covariance.push(4444.0);
let msg_native2 =
WrappedNativeMsg::<geometry_msgs::msg::AccelWithCovariance>::from(&msg);
let msg2 = geometry_msgs::msg::AccelWithCovariance::from_native(&msg_native2);
println!("{:#?}", msg2);
}
}
#[test]
#[should_panic] // we are testing that we cannot have to many elements in a fixed sized field
fn test_capped_sequence() -> () {
// float64[<=3] dimensions in the .msg translates to a float64 sequence AND an array size field. handle it...
unsafe {
let x = rosidl_typesupport_introspection_c__get_message_type_support_handle__shape_msgs__msg__SolidPrimitive();
let members = (*x).data as *const rosidl_typesupport_introspection_c__MessageMembers;
println!("{:#?}", *members);
let memberslice =
std::slice::from_raw_parts((*members).members_, (*members).member_count_ as usize);
for member in memberslice {
println!("member: {:#?}", *member);
}
let msg_native = WrappedNativeMsg::<shape_msgs::msg::SolidPrimitive>::new();
let mut msg = shape_msgs::msg::SolidPrimitive::from_native(&msg_native);
println!("{:#?}", msg);
msg.dimensions.push(1.0);
msg.dimensions.push(1.0);
msg.dimensions.push(1.0);
msg.dimensions.push(1.0); // only three elements allowed
let _msg_native2 = WrappedNativeMsg::<shape_msgs::msg::SolidPrimitive>::from(&msg);
}
}
#[test]
fn test_generation_string_use() -> () {
let msg = std_msgs::msg::String { data: "hej".into() };
let msg2 = msg.clone();
let msg_native = WrappedNativeMsg::<std_msgs::msg::String>::from(&msg2);
let msg2 = std_msgs::msg::String::from_native(&msg_native);
assert_eq!(msg, msg2)
}
#[test]
fn test_generation_bool_use() -> () {
let msg = std_msgs::msg::Bool { data: true };
let msg_native = WrappedNativeMsg::<std_msgs::msg::Bool>::from(&msg);
let msg2 = std_msgs::msg::Bool::from_native(&msg_native);
assert_eq!(msg, msg2);
}
#[test]
fn test_float_sequence() -> () {
use trajectory_msgs::msg::*;
let native = WrappedNativeMsg::<JointTrajectoryPoint>::new();
let mut msg = JointTrajectoryPoint::from_native(&native);
msg.positions.push(39.0);
msg.positions.push(34.0);
let new_native = WrappedNativeMsg::<JointTrajectoryPoint>::from(&msg);
let new_msg = JointTrajectoryPoint::from_native(&new_native);
println!("{:#?}", new_msg);
assert_eq!(msg, new_msg);
}
#[test]
fn test_deault() -> () {
use trajectory_msgs::msg::*;
let mut msg: JointTrajectoryPoint = Default::default();
msg.positions.push(39.0);
msg.positions.push(34.0);
let mut new_native = WrappedNativeMsg::<JointTrajectoryPoint>::from(&msg);
unsafe { *((*new_native).positions.data) = 88.9 };
let new_msg = JointTrajectoryPoint::from_native(&new_native);
println!("{:#?}", new_msg);
assert_ne!(msg, new_msg);
}
#[test]
fn test_untyped_json() -> () {
let mut msg = trajectory_msgs::msg::JointTrajectoryPoint::default();
msg.positions.push(39.0);
msg.positions.push(34.0);
let json = serde_json::to_value(msg.clone()).unwrap();
let mut native =
WrappedNativeMsgUntyped::new_from("trajectory_msgs/msg/JointTrajectoryPoint").unwrap();
native.from_json(json.clone()).unwrap();
let json2 = native.to_json().unwrap();
assert_eq!(json, json2);
let msg2: trajectory_msgs::msg::JointTrajectoryPoint =
serde_json::from_value(json2).unwrap();
assert_eq!(msg, msg2);
}
#[cfg(r2r__test_msgs__msg__Arrays)]
#[test]
fn test_test_msgs_array() -> () {
let mut msg = test_msgs::msg::Arrays::default();
println!("msg: {:?}", msg.string_values);
msg.string_values = vec!["hej".to_string(), "hopp".to_string(), "stropp".to_string()];
let msg_native = WrappedNativeMsg::<test_msgs::msg::Arrays>::from(&msg);
let msg2 = test_msgs::msg::Arrays::from_native(&msg_native);
assert_eq!(msg, msg2);
}
#[cfg(r2r__test_msgs__msg__Arrays)]
#[test]
#[should_panic]
fn test_test_msgs_array_too_few_elems() -> () {
let mut msg = test_msgs::msg::Arrays::default();
println!("msg: {:?}", msg.string_values);
msg.string_values = vec!["hej".to_string(), "hopp".to_string()];
let _msg_native = WrappedNativeMsg::<test_msgs::msg::Arrays>::from(&msg);
}
#[cfg(r2r__test_msgs__msg__WStrings)]
#[test]
fn test_test_msgs_wstring() -> () {
let mut msg = test_msgs::msg::WStrings::default();
let rust_str = "ハローワールド";
msg.wstring_value = rust_str.to_string();
let native = WrappedNativeMsg::<test_msgs::msg::WStrings>::from(&msg);
println!("msg: {:?}", msg);
let msg2 = test_msgs::msg::WStrings::from_native(&native);
assert_eq!(msg.wstring_value, msg2.wstring_value);
}
#[cfg(r2r__example_interfaces__srv__AddTwoInts)]
#[test]
fn test_service_msgs() {
use example_interfaces::srv::AddTwoInts;
let mut req = AddTwoInts::Request::default();
req.a = 5;
let rn = WrappedNativeMsg::<_>::from(&req);
let req2 = AddTwoInts::Request::from_native(&rn);
println!("req2 {:?}", req2);
assert_eq!(req, req2);
let mut resp = AddTwoInts::Response::default();
resp.sum = 5;
let rn = WrappedNativeMsg::<_>::from(&resp);
let resp2 = AddTwoInts::Response::from_native(&rn);
println!("resp {:?}", resp2);
assert_eq!(resp, resp2);
}
#[cfg(r2r__std_srvs__srv__Empty)]
#[test]
fn test_empty_msgs() {
use std_srvs::srv::Empty;
let req = Empty::Request::default();
let resp = Empty::Response::default();
println!("req {:?}", req);
println!("resp {:?}", resp);
}
#[cfg(r2r__example_interfaces__action__Fibonacci)]
#[test]
fn test_action_msgs() {
use example_interfaces::action::Fibonacci;
let mut goal = Fibonacci::Goal::default();
goal.order = 5;
let gn = WrappedNativeMsg::<_>::from(&goal);
let goal2 = Fibonacci::Goal::from_native(&gn);
println!("goal2 {:?}", goal2);
assert_eq!(goal, goal2);
let mut res = Fibonacci::Result::default();
res.sequence = vec![1, 2, 3];
let rn = WrappedNativeMsg::<_>::from(&res);
let res2 = Fibonacci::Result::from_native(&rn);
println!("res2 {:?}", res2);
assert_eq!(res, res2);
let mut fb = Fibonacci::Feedback::default();
fb.sequence = vec![4, 3, 6];
let fbn = WrappedNativeMsg::<_>::from(&fb);
let fb2 = Fibonacci::Feedback::from_native(&fbn);
println!("feedback2 {:?}", fb2);
assert_eq!(fb, fb2);
let fb = WrappedNativeMsg::<Fibonacci::Feedback>::new();
let fb1 = Fibonacci::Feedback::default();
let fb2 = Fibonacci::Feedback::from_native(&fb);
assert_eq!(fb1, fb2);
}
#[cfg(r2r__example_interfaces__srv__AddTwoInts)]
#[test]
fn test_untyped_service_support() {
let ts = UntypedServiceSupport::new_from("example_interfaces/srv/AddTwoInts").unwrap();
let msg = (ts.make_request_msg)();
let json = msg.to_json();
// the message should contain something (default msg)
assert!(!json.unwrap().to_string().is_empty());
}
#[cfg(r2r__example_interfaces__action__Fibonacci)]
#[test]
fn test_untyped_action_support() {
use example_interfaces::action::Fibonacci;
let ts = UntypedActionSupport::new_from("example_interfaces/action/Fibonacci").unwrap();
let uuid = unique_identifier_msgs::msg::UUID::default();
let goal = Fibonacci::Goal { order: 5 };
let json_goal = serde_json::to_value(&goal).unwrap();
let json_request = (ts.make_goal_request_msg)(uuid, json_goal)
.to_json()
.unwrap();
// the message should contain something (default msg)
assert!(!json_request.to_string().is_empty());
}
}

896
src/nodes.rs Normal file
View File

@ -0,0 +1,896 @@
use super::*;
pub struct Node {
context: Context,
pub params: HashMap<String, ParameterValue>,
node_handle: Box<rcl_node_t>,
// the node owns the subscribers
subs: Vec<Box<dyn Subscriber_>>,
// services,
services: Vec<Arc<Mutex<dyn Service_>>>,
// service clients
clients: Vec<Arc<Mutex<dyn Client_>>>,
// action clients
action_clients: Vec<Arc<Mutex<dyn ActionClient_>>>,
// action servers
action_servers: Vec<Arc<Mutex<dyn ActionServer_>>>,
// timers,
timers: Vec<Timer_>,
// and the publishers, whom we allow to be shared.. hmm.
pubs: Vec<Arc<rcl_publisher_t>>,
}
unsafe impl Send for Node {}
impl Node {
pub fn name(&self) -> Result<String> {
let cstr = unsafe { rcl_node_get_name(self.node_handle.as_ref()) };
if cstr == std::ptr::null() {
return Err(Error::RCL_RET_NODE_INVALID);
}
let s = unsafe { CStr::from_ptr(cstr) };
Ok(s.to_str().unwrap_or("").to_owned())
}
pub fn fully_qualified_name(&self) -> Result<String> {
let cstr = unsafe { rcl_node_get_fully_qualified_name(self.node_handle.as_ref()) };
if cstr == std::ptr::null() {
return Err(Error::RCL_RET_NODE_INVALID);
}
let s = unsafe { CStr::from_ptr(cstr) };
Ok(s.to_str().unwrap_or("").to_owned())
}
pub fn namespace(&self) -> Result<String> {
let cstr = unsafe { rcl_node_get_namespace(self.node_handle.as_ref()) };
if cstr == std::ptr::null() {
return Err(Error::RCL_RET_NODE_INVALID);
}
let s = unsafe { CStr::from_ptr(cstr) };
Ok(s.to_str().unwrap_or("").to_owned())
}
fn load_params(&mut self) -> Result<()> {
let ctx = self.context.context_handle.lock().unwrap();
let mut params: Box<*mut rcl_params_t> = Box::new(std::ptr::null_mut());
let ret =
unsafe { rcl_arguments_get_param_overrides(&ctx.global_arguments, params.as_mut()) };
if ret != RCL_RET_OK as i32 {
eprintln!("could not read parameters: {}", ret);
return Err(Error::from_rcl_error(ret));
}
if *params == std::ptr::null_mut() {
return Ok(());
}
let node_names = unsafe {
std::slice::from_raw_parts(
(*(*params.as_ref())).node_names,
(*(*params.as_ref())).num_nodes,
)
};
let node_params = unsafe {
std::slice::from_raw_parts(
(*(*params.as_ref())).params,
(*(*params.as_ref())).num_nodes,
)
};
let qualified_name = self.fully_qualified_name()?;
let name = self.name()?;
for (nn, np) in node_names.iter().zip(node_params) {
let node_name_cstr = unsafe { CStr::from_ptr(*nn) };
let node_name = node_name_cstr.to_str().unwrap_or("");
// This is copied from rclcpp, but there is a comment there suggesting
// that more wildcards will be added in the future. Take note and mimic
// their behavior.
if !(node_name == "/**"
|| node_name == "**"
|| qualified_name == node_name
|| name == node_name)
{
continue;
}
// make key value pairs.
let param_names =
unsafe { std::slice::from_raw_parts(np.parameter_names, np.num_params) };
let param_values =
unsafe { std::slice::from_raw_parts(np.parameter_values, np.num_params) };
for (s, v) in param_names.iter().zip(param_values) {
let s = unsafe { CStr::from_ptr(*s) };
let key = s.to_str().unwrap_or("");
let val = parameter_value_from_rcl(&*v);
self.params.insert(key.to_owned(), val);
}
}
unsafe { rcl_yaml_node_struct_fini(*params) };
Ok(())
}
pub fn create(ctx: Context, name: &str, namespace: &str) -> Result<Node> {
let (res, node_handle) = {
let mut ctx_handle = ctx.context_handle.lock().unwrap();
let c_node_name = CString::new(name).unwrap();
let c_node_ns = CString::new(namespace).unwrap();
let mut node_handle: Box<rcl_node_t> =
unsafe { Box::new(rcl_get_zero_initialized_node()) };
let res = unsafe {
let node_options = rcl_node_get_default_options();
rcl_node_init(
node_handle.as_mut(),
c_node_name.as_ptr(),
c_node_ns.as_ptr(),
ctx_handle.as_mut(),
&node_options as *const _,
)
};
(res, node_handle)
};
if res == RCL_RET_OK as i32 {
let mut node = Node {
params: HashMap::new(),
context: ctx,
node_handle,
subs: Vec::new(),
services: Vec::new(),
clients: Vec::new(),
action_clients: Vec::new(),
action_servers: Vec::new(),
timers: Vec::new(),
pubs: Vec::new(),
};
node.load_params()?;
Ok(node)
} else {
eprintln!("could not create node{}", res);
Err(Error::from_rcl_error(res))
}
}
pub fn subscribe<T: 'static>(&mut self, topic: &str) -> Result<impl Stream<Item = T> + Unpin>
where
T: WrappedTypesupport,
{
let subscription_handle =
create_subscription_helper(self.node_handle.as_mut(), topic, T::get_ts())?;
let (sender, receiver) = mpsc::channel::<T>(10);
let ws = TypedSubscriber {
rcl_handle: subscription_handle,
sender,
};
self.subs.push(Box::new(ws));
Ok(receiver)
}
pub fn subscribe_native<T: 'static>(
&mut self,
topic: &str,
) -> Result<impl Stream<Item = WrappedNativeMsg<T>> + Unpin>
where
T: WrappedTypesupport,
{
let subscription_handle =
create_subscription_helper(self.node_handle.as_mut(), topic, T::get_ts())?;
let (sender, receiver) = mpsc::channel::<WrappedNativeMsg<T>>(10);
let ws = NativeSubscriber {
rcl_handle: subscription_handle,
sender,
};
self.subs.push(Box::new(ws));
Ok(receiver)
}
// Its not really untyped since we know the underlying type... But we throw this info away :)
pub fn subscribe_untyped(
&mut self,
topic: &str,
topic_type: &str,
) -> Result<impl Stream<Item = Result<serde_json::Value>> + Unpin> {
let msg = WrappedNativeMsgUntyped::new_from(topic_type)?;
let subscription_handle =
create_subscription_helper(self.node_handle.as_mut(), topic, msg.ts)?;
let (sender, receiver) = mpsc::channel::<Result<serde_json::Value>>(10);
let ws = UntypedSubscriber {
rcl_handle: subscription_handle,
topic_type: topic_type.to_string(),
sender,
};
self.subs.push(Box::new(ws));
Ok(receiver)
}
pub fn create_service<T: 'static>(
&mut self,
service_name: &str,
) -> Result<impl Stream<Item = ServiceRequest<T>> + Unpin>
where
T: WrappedServiceTypeSupport,
{
let service_handle =
create_service_helper(self.node_handle.as_mut(), service_name, T::get_ts())?;
let (sender, receiver) = mpsc::channel::<ServiceRequest<T>>(10);
let ws = TypedService::<T> {
rcl_handle: service_handle,
outstanding_requests: vec![],
sender,
};
self.services.push(Arc::new(Mutex::new(ws)));
Ok(receiver)
}
pub fn create_client<T: 'static>(&mut self, service_name: &str) -> Result<Client<T>>
where
T: WrappedServiceTypeSupport,
{
let client_handle =
create_client_helper(self.node_handle.as_mut(), service_name, T::get_ts())?;
let ws = TypedClient::<T> {
rcl_handle: client_handle,
response_channels: Vec::new(),
poll_available_channels: Vec::new(),
};
let client_arc = Arc::new(Mutex::new(ws));
let c = make_client(Arc::downgrade(&client_arc));
self.clients.push(client_arc);
Ok(c)
}
/// Create a service client without having the concrete rust type.
pub fn create_client_untyped(
&mut self,
service_name: &str,
service_type: &str,
) -> Result<UntypedClient> {
let service_type = UntypedServiceSupport::new_from(service_type)?;
let client_handle =
create_client_helper(self.node_handle.as_mut(), service_name, service_type.ts)?;
let client = UntypedClient_ {
service_type,
rcl_handle: client_handle,
response_channels: Vec::new(),
poll_available_channels: Vec::new(),
};
let client_arc = Arc::new(Mutex::new(client));
let c = make_untyped_client(Arc::downgrade(&client_arc));
self.clients.push(client_arc);
Ok(c)
}
pub fn is_available(
&mut self,
client: &dyn IsAvailablePollable,
) -> Result<impl Future<Output = Result<()>>> {
let (sender, receiver) = oneshot::channel();
client.register_poll_available(sender)?;
Ok(receiver.map_err(|_| Error::RCL_RET_CLIENT_INVALID))
}
pub fn create_action_client<T: 'static>(&mut self, action_name: &str) -> Result<ActionClient<T>>
where
T: WrappedActionTypeSupport,
{
let client_handle =
create_action_client_helper(self.node_handle.as_mut(), action_name, T::get_ts())?;
let client = WrappedActionClient::<T> {
rcl_handle: client_handle,
goal_response_channels: Vec::new(),
cancel_response_channels: Vec::new(),
feedback_senders: Vec::new(),
result_senders: Vec::new(),
result_requests: Vec::new(),
goal_status: HashMap::new(),
poll_available_channels: Vec::new(),
};
let client_arc = Arc::new(Mutex::new(client));
self.action_clients.push(client_arc.clone());
let c = make_action_client(Arc::downgrade(&client_arc));
Ok(c)
}
/// Create an action client without having the concrete rust type.
pub fn create_action_client_untyped(
&mut self,
action_name: &str,
action_type: &str,
) -> Result<ActionClientUntyped> {
let action_type_support = UntypedActionSupport::new_from(action_type)?;
let client_handle = create_action_client_helper(
self.node_handle.as_mut(),
action_name,
action_type_support.ts,
)?;
let client = WrappedActionClientUntyped {
action_type_support,
rcl_handle: client_handle,
goal_response_channels: Vec::new(),
cancel_response_channels: Vec::new(),
feedback_senders: Vec::new(),
result_senders: Vec::new(),
result_requests: Vec::new(),
goal_status: HashMap::new(),
poll_available_channels: Vec::new(),
};
let client_arc = Arc::new(Mutex::new(client));
self.action_clients.push(client_arc.clone());
let c = make_action_client_untyped(Arc::downgrade(&client_arc));
Ok(c)
}
pub fn create_action_server<T: 'static>(
&mut self,
action_name: &str,
) -> Result<impl Stream<Item = GoalRequest<T>> + Unpin>
where
T: WrappedActionTypeSupport,
{
// for now automatically create a ros clock...
let mut clock_handle = MaybeUninit::<rcl_clock_t>::uninit();
let ret = unsafe {
rcl_ros_clock_init(
clock_handle.as_mut_ptr(),
&mut rcutils_get_default_allocator(),
)
};
if ret != RCL_RET_OK as i32 {
eprintln!("could not create steady clock: {}", ret);
return Err(Error::from_rcl_error(ret));
}
let mut clock_handle = Box::new(unsafe { clock_handle.assume_init() });
let (goal_request_sender, goal_request_receiver) = mpsc::channel::<GoalRequest<T>>(10);
let server_handle = create_action_server_helper(
self.node_handle.as_mut(),
action_name,
clock_handle.as_mut(),
T::get_ts(),
)?;
let server = WrappedActionServer::<T> {
rcl_handle: server_handle,
clock_handle,
goal_request_sender,
active_cancel_requests: Vec::new(),
cancel_senders: HashMap::new(),
goals: HashMap::new(),
result_msgs: HashMap::new(),
result_requests: HashMap::new(),
};
let server_arc = Arc::new(Mutex::new(server));
self.action_servers.push(server_arc);
Ok(goal_request_receiver)
}
pub fn create_publisher<T>(&mut self, topic: &str) -> Result<Publisher<T>>
where
T: WrappedTypesupport,
{
let publisher_handle =
create_publisher_helper(self.node_handle.as_mut(), topic, T::get_ts())?;
let arc = Arc::new(publisher_handle);
let p = make_publisher(Arc::downgrade(&arc));
self.pubs.push(arc);
Ok(p)
}
pub fn create_publisher_untyped(
&mut self,
topic: &str,
topic_type: &str,
) -> Result<PublisherUntyped> {
let dummy = WrappedNativeMsgUntyped::new_from(topic_type)?;
let publisher_handle = create_publisher_helper(self.node_handle.as_mut(), topic, dummy.ts)?;
let arc = Arc::new(publisher_handle);
let p = make_publisher_untyped(Arc::downgrade(&arc), topic_type.to_owned());
self.pubs.push(arc);
Ok(p)
}
pub fn spin_once(&mut self, timeout: Duration) {
// first handle any completed action cancellation responses
for a in &mut self.action_servers {
a.lock().unwrap().send_completed_cancel_requests();
}
// as well as polling any services/action servers for availability
for c in &mut self.clients {
c.lock().unwrap().poll_available(self.node_handle.as_mut());
}
for c in &mut self.action_clients {
c.lock().unwrap().poll_available(self.node_handle.as_mut());
}
let timeout = timeout.as_nanos() as i64;
let mut ws = unsafe { rcl_get_zero_initialized_wait_set() };
// #[doc = "* This function is thread-safe for unique wait sets with unique contents."]
// #[doc = "* This function cannot operate on the same wait set in multiple threads, and"]
// #[doc = "* the wait sets may not share content."]
// #[doc = "* For example, calling rcl_wait() in two threads on two different wait sets"]
// #[doc = "* that both contain a single, shared guard condition is undefined behavior."]
// count action client wait set needs
let mut total_action_subs = 0;
let mut total_action_clients = 0;
for c in &self.action_clients {
let mut num_subs = 0;
let mut num_gc = 0;
let mut num_timers = 0;
let mut num_clients = 0;
let mut num_services = 0;
action_client_get_num_waits(
c.lock().unwrap().handle(),
&mut num_subs,
&mut num_gc,
&mut num_timers,
&mut num_clients,
&mut num_services,
)
.expect("could not get action client wait sets");
// sanity check
assert_eq!(num_subs, 2);
assert_eq!(num_clients, 3);
assert_eq!(num_gc, 0);
assert_eq!(num_timers, 0);
assert_eq!(num_services, 0);
total_action_subs += num_subs;
total_action_clients += num_clients;
}
// count action server wait set needs
let mut total_action_timers = 0;
let mut total_action_services = 0;
for s in &self.action_servers {
let mut num_subs = 0;
let mut num_gc = 0;
let mut num_timers = 0;
let mut num_clients = 0;
let mut num_services = 0;
action_server_get_num_waits(
s.lock().unwrap().handle(),
&mut num_subs,
&mut num_gc,
&mut num_timers,
&mut num_clients,
&mut num_services,
)
.expect("could not get action client wait sets");
// sanity check
assert_eq!(num_subs, 0);
assert_eq!(num_clients, 0);
assert_eq!(num_gc, 0);
assert_eq!(num_timers, 1);
assert_eq!(num_services, 3);
total_action_timers += num_timers;
total_action_services += num_services;
}
{
let mut ctx = self.context.context_handle.lock().unwrap();
unsafe {
rcl_wait_set_init(
&mut ws,
self.subs.len() + total_action_subs,
0,
self.timers.len() + total_action_timers,
self.clients.len() + total_action_clients,
self.services.len() + total_action_services,
0,
ctx.as_mut(),
rcutils_get_default_allocator(),
);
}
}
unsafe {
rcl_wait_set_clear(&mut ws);
}
for s in &self.subs {
unsafe {
rcl_wait_set_add_subscription(&mut ws, s.handle(), std::ptr::null_mut());
}
}
for s in &self.timers {
unsafe {
rcl_wait_set_add_timer(&mut ws, &s.timer_handle, std::ptr::null_mut());
}
}
for s in &self.clients {
unsafe {
rcl_wait_set_add_client(&mut ws, s.lock().unwrap().handle(), std::ptr::null_mut());
}
}
for s in &self.services {
unsafe {
rcl_wait_set_add_service(&mut ws, s.lock().unwrap().handle(), std::ptr::null_mut());
}
}
// code (further) below assumes that actions are added last... perhaps a
// bad assumption. e.g. we add subscriptions and timers of
// the node before ones created automatically by actions. we
// then assume that we can count on the waitables created by
// the actions are added at the end of the wait set arrays
for ac in &self.action_clients {
unsafe {
rcl_action_wait_set_add_action_client(
&mut ws,
ac.lock().unwrap().handle(),
std::ptr::null_mut(),
std::ptr::null_mut(),
);
}
}
for acs in &self.action_servers {
unsafe {
rcl_action_wait_set_add_action_server(
&mut ws,
acs.lock().unwrap().handle(),
std::ptr::null_mut(),
);
}
}
let ret = unsafe { rcl_wait(&mut ws, timeout) };
if ret == RCL_RET_TIMEOUT as i32 {
unsafe {
rcl_wait_set_fini(&mut ws);
}
return;
}
let ws_subs = unsafe { std::slice::from_raw_parts(ws.subscriptions, self.subs.len()) };
for (s, ws_s) in self.subs.iter_mut().zip(ws_subs) {
if ws_s != &std::ptr::null() {
s.handle_incoming();
}
}
let ws_timers = unsafe { std::slice::from_raw_parts(ws.timers, self.timers.len()) };
let mut timers_to_remove = vec![];
for (s, ws_s) in self.timers.iter_mut().zip(ws_timers) {
if ws_s != &std::ptr::null() {
let mut is_ready = false;
let ret = unsafe { rcl_timer_is_ready(&s.timer_handle, &mut is_ready) };
if ret == RCL_RET_OK as i32 {
if is_ready {
let mut nanos = 0i64;
// todo: error handling
let ret = unsafe {
rcl_timer_get_time_since_last_call(&s.timer_handle, &mut nanos)
};
if ret == RCL_RET_OK as i32 {
let ret = unsafe { rcl_timer_call(&mut s.timer_handle) };
if ret == RCL_RET_OK as i32 {
match s.sender.try_send(Duration::from_nanos(nanos as u64)) {
Err(e) => {
if e.is_full() {
println!("Warning: timer tick not handled in time - no wakeup will occur");
}
if e.is_disconnected() {
// client dropped the timer handle, let's drop our timer as well.
timers_to_remove.push(s.timer_handle);
}
}
_ => {} // ok
}
}
}
}
}
}
}
// drop timers scheduled for deletion
self.timers
.retain(|t| !timers_to_remove.contains(&t.timer_handle));
let ws_clients = unsafe { std::slice::from_raw_parts(ws.clients, self.clients.len()) };
for (s, ws_s) in self.clients.iter_mut().zip(ws_clients) {
if ws_s != &std::ptr::null() {
let mut s = s.lock().unwrap();
s.handle_response();
}
}
let ws_services = unsafe { std::slice::from_raw_parts(ws.services, self.services.len()) };
for (s, ws_s) in self.services.iter_mut().zip(ws_services) {
if ws_s != &std::ptr::null() {
let mut service = s.lock().unwrap();
service.handle_request(s.clone());
}
}
for ac in &self.action_clients {
let mut is_feedback_ready = false;
let mut is_status_ready = false;
let mut is_goal_response_ready = false;
let mut is_cancel_response_ready = false;
let mut is_result_response_ready = false;
let ret = unsafe {
rcl_action_client_wait_set_get_entities_ready(
&ws,
ac.lock().unwrap().handle(),
&mut is_feedback_ready,
&mut is_status_ready,
&mut is_goal_response_ready,
&mut is_cancel_response_ready,
&mut is_result_response_ready,
)
};
if ret != RCL_RET_OK as i32 {
continue;
}
if is_feedback_ready {
let mut acs = ac.lock().unwrap();
acs.handle_feedback_msg();
}
if is_status_ready {
let mut acs = ac.lock().unwrap();
acs.handle_status_msg();
}
if is_goal_response_ready {
let mut acs = ac.lock().unwrap();
acs.handle_goal_response();
}
if is_cancel_response_ready {
let mut acs = ac.lock().unwrap();
acs.handle_cancel_response();
}
if is_result_response_ready {
let mut acs = ac.lock().unwrap();
acs.handle_result_response();
}
}
for s in &self.action_servers {
let mut is_goal_request_ready = false;
let mut is_cancel_request_ready = false;
let mut is_result_request_ready = false;
let mut is_goal_expired = false;
let ret = unsafe {
rcl_action_server_wait_set_get_entities_ready(
&ws,
s.lock().unwrap().handle(),
&mut is_goal_request_ready,
&mut is_cancel_request_ready,
&mut is_result_request_ready,
&mut is_goal_expired,
)
};
if ret != RCL_RET_OK as i32 {
continue;
}
if is_goal_request_ready {
let mut acs = s.lock().unwrap();
acs.handle_goal_request(s.clone());
}
if is_cancel_request_ready {
let mut acs = s.lock().unwrap();
acs.handle_cancel_request();
}
if is_result_request_ready {
let mut acs = s.lock().unwrap();
acs.handle_result_request();
}
if is_goal_expired {
let mut acs = s.lock().unwrap();
acs.handle_goal_expired();
}
}
unsafe {
rcl_wait_set_fini(&mut ws);
}
}
pub fn get_topic_names_and_types(&self) -> Result<HashMap<String, Vec<String>>> {
let mut tnat = unsafe { rmw_get_zero_initialized_names_and_types() };
let ret = unsafe {
rcl_get_topic_names_and_types(
self.node_handle.as_ref(),
&mut rcutils_get_default_allocator(),
false,
&mut tnat,
)
};
if ret != RCL_RET_OK as i32 {
eprintln!("could not get topic names and types {}", ret);
return Err(Error::from_rcl_error(ret));
}
let names = unsafe { std::slice::from_raw_parts(tnat.names.data, tnat.names.size) };
let types = unsafe { std::slice::from_raw_parts(tnat.types, tnat.names.size) };
let mut res = HashMap::new();
for (n, t) in names.iter().zip(types) {
let topic_name = unsafe { CStr::from_ptr(*n).to_str().unwrap().to_owned() };
let topic_types = unsafe { std::slice::from_raw_parts(t, t.size) };
let topic_types: Vec<String> = unsafe {
topic_types
.iter()
.map(|t| CStr::from_ptr(*((*t).data)).to_str().unwrap().to_owned())
.collect()
};
res.insert(topic_name, topic_types);
}
unsafe {
rmw_names_and_types_fini(&mut tnat);
} // TODO: check return value
Ok(res)
}
pub fn create_wall_timer(&mut self, period: Duration) -> Result<Timer> {
let mut clock_handle = MaybeUninit::<rcl_clock_t>::uninit();
let ret = unsafe {
rcl_steady_clock_init(
clock_handle.as_mut_ptr(),
&mut rcutils_get_default_allocator(),
)
};
if ret != RCL_RET_OK as i32 {
eprintln!("could not create steady clock: {}", ret);
return Err(Error::from_rcl_error(ret));
}
let mut clock_handle = Box::new(unsafe { clock_handle.assume_init() });
let mut timer_handle = unsafe { rcl_get_zero_initialized_timer() };
{
let mut ctx = self.context.context_handle.lock().unwrap();
let ret = unsafe {
rcl_timer_init(
&mut timer_handle,
clock_handle.as_mut(),
ctx.as_mut(),
period.as_nanos() as i64,
None,
rcutils_get_default_allocator(),
)
};
if ret != RCL_RET_OK as i32 {
eprintln!("could not create timer: {}", ret);
return Err(Error::from_rcl_error(ret));
}
}
let (tx, rx) = mpsc::channel::<Duration>(1);
let timer = Timer_ {
timer_handle,
clock_handle,
sender: tx,
};
self.timers.push(timer);
let out_timer = Timer { receiver: rx };
Ok(out_timer)
}
pub fn logger<'a>(&'a self) -> &'a str {
let ptr = unsafe { rcl_node_get_logger_name(self.node_handle.as_ref()) };
if ptr == std::ptr::null() {
return "";
}
let s = unsafe { CStr::from_ptr(ptr) };
s.to_str().unwrap_or("")
}
}
struct Timer_ {
timer_handle: rcl_timer_t,
clock_handle: Box<rcl_clock_t>,
sender: mpsc::Sender<Duration>,
}
pub struct Timer {
receiver: mpsc::Receiver<Duration>,
}
impl Timer {
pub async fn tick(&mut self) -> Result<Duration> {
let next = self.receiver.next().await;
if let Some(elapsed) = next {
Ok(elapsed)
} else {
Err(Error::RCL_RET_TIMER_INVALID)
}
}
}
// Since publishers are temporarily upgraded to owners during the
// actual publish but are not the ones that handle cleanup, we simply
// wait until there are no other owners in the cleanup procedure. The
// next time a publisher wants to publish they will fail because the
// value in the Arc has been dropped. Hacky but works.
fn wait_until_unwrapped<T>(mut a: Arc<T>) -> T {
loop {
match Arc::try_unwrap(a) {
Ok(b) => return b,
Err(t) => a = t,
}
}
}
impl Drop for Node {
fn drop(&mut self) {
// fini functions are not thread safe so lock the context.
let _ctx_handle = self.context.context_handle.lock().unwrap();
for s in &mut self.subs {
s.destroy(&mut self.node_handle);
}
for s in &mut self.services {
s.lock().unwrap().destroy(&mut self.node_handle);
}
for t in &mut self.timers {
// TODO: check return values
let _ret = unsafe { rcl_timer_fini(&mut t.timer_handle) };
// TODO: allow other types of clocks...
let _ret = unsafe { rcl_steady_clock_fini(t.clock_handle.as_mut()) };
}
for c in &mut self.action_clients {
c.lock().unwrap().destroy(&mut self.node_handle);
}
for s in &mut self.action_servers {
s.lock().unwrap().destroy(&mut self.node_handle);
}
while let Some(p) = self.pubs.pop() {
let mut p = wait_until_unwrapped(p);
let _ret = unsafe { rcl_publisher_fini(&mut p as *mut _, self.node_handle.as_mut()) };
// TODO: check ret
}
unsafe {
rcl_node_fini(self.node_handle.as_mut());
}
}
}
pub trait IsAvailablePollable {
fn register_poll_available(&self, sender: oneshot::Sender<()>) -> Result<()>;
}

66
src/parameters.rs Normal file
View File

@ -0,0 +1,66 @@
use super::*;
#[derive(Debug, PartialEq, Clone)]
pub enum ParameterValue {
NotSet,
Bool(bool),
Integer(i64),
Double(f64),
String(String),
BoolArray(Vec<bool>),
ByteArray(Vec<u8>),
IntegerArray(Vec<i64>),
DoubleArray(Vec<f64>),
StringArray(Vec<String>),
}
pub fn parameter_value_from_rcl(v: &rcl_variant_t) -> ParameterValue {
if v.bool_value != std::ptr::null_mut() {
ParameterValue::Bool(unsafe { *v.bool_value })
} else if v.integer_value != std::ptr::null_mut() {
ParameterValue::Integer(unsafe { *v.integer_value })
} else if v.double_value != std::ptr::null_mut() {
ParameterValue::Double(unsafe { *v.double_value })
} else if v.string_value != std::ptr::null_mut() {
let s = unsafe { CStr::from_ptr(v.string_value) };
let string = s.to_str().unwrap_or("").to_owned();
ParameterValue::String(string)
} else if v.byte_array_value != std::ptr::null_mut() {
let vals = unsafe {
std::slice::from_raw_parts((*v.byte_array_value).values, (*v.byte_array_value).size)
};
ParameterValue::ByteArray(vals.iter().cloned().collect())
} else if v.bool_array_value != std::ptr::null_mut() {
let vals = unsafe {
std::slice::from_raw_parts((*v.bool_array_value).values, (*v.bool_array_value).size)
};
ParameterValue::BoolArray(vals.iter().cloned().collect())
} else if v.integer_array_value != std::ptr::null_mut() {
let vals = unsafe {
std::slice::from_raw_parts(
(*v.integer_array_value).values,
(*v.integer_array_value).size,
)
};
ParameterValue::IntegerArray(vals.iter().cloned().collect())
} else if v.double_array_value != std::ptr::null_mut() {
let vals = unsafe {
std::slice::from_raw_parts((*v.double_array_value).values, (*v.double_array_value).size)
};
ParameterValue::DoubleArray(vals.iter().cloned().collect())
} else if v.string_array_value != std::ptr::null_mut() {
let vals = unsafe {
std::slice::from_raw_parts((*v.string_array_value).data, (*v.string_array_value).size)
};
let s = vals
.iter()
.map(|cs| {
let s = unsafe { CStr::from_ptr(*cs) };
s.to_str().unwrap_or("").to_owned()
})
.collect();
ParameterValue::StringArray(s)
} else {
ParameterValue::NotSet
}
}

163
src/publishers.rs Normal file
View File

@ -0,0 +1,163 @@
use super::*;
// The publish function is thread safe. ROS2 docs state:
// =============
//
// This function is thread safe so long as access to both the
// publisher and the" `ros_message` is synchronized." That means that
// calling rcl_publish() from multiple threads is allowed, but"
// calling rcl_publish() at the same time as non-thread safe
// publisher" functions is not, e.g. calling rcl_publish() and
// rcl_publisher_fini()" concurrently is not allowed." Before calling
// rcl_publish() the message can change and after calling"
// rcl_publish() the message can change, but it cannot be changed
// during the" publish call." The same `ros_message`, however, can be
// passed to multiple calls of" rcl_publish() simultaneously, even if
// the publishers differ." The `ros_message` is unmodified by
// rcl_publish()."
//
// TODO: I guess there is a potential error source in destructuring
// while calling publish. I don't think its worth to protect with a
// mutex/rwlock for this though...
//
// Methods that mutate need to called from the thread owning the Node.
// I don't think we can count on Node being generally thread-safe.
// So keep pub/sub management and polling contained to one thread
// and send out publishers.
unsafe impl<T> Send for Publisher<T> where T: WrappedTypesupport {}
#[derive(Debug, Clone)]
pub struct Publisher<T>
where
T: WrappedTypesupport,
{
handle: Weak<rcl_publisher_t>,
type_: PhantomData<T>,
}
unsafe impl Send for PublisherUntyped {}
#[derive(Debug, Clone)]
pub struct PublisherUntyped {
handle: Weak<rcl_publisher_t>,
type_: String,
}
pub fn make_publisher<T>(handle: Weak<rcl_publisher_t>) -> Publisher<T>
where
T: WrappedTypesupport,
{
Publisher {
handle,
type_: PhantomData,
}
}
pub fn make_publisher_untyped(handle: Weak<rcl_publisher_t>, type_: String) -> PublisherUntyped {
PublisherUntyped { handle, type_ }
}
pub fn create_publisher_helper(
node: &mut rcl_node_t,
topic: &str,
typesupport: *const rosidl_message_type_support_t,
) -> Result<rcl_publisher_t> {
let mut publisher_handle = unsafe { rcl_get_zero_initialized_publisher() };
let topic_c_string = CString::new(topic).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
let result = unsafe {
let mut publisher_options = rcl_publisher_get_default_options();
publisher_options.qos = rmw_qos_profile_t::default();
rcl_publisher_init(
&mut publisher_handle,
node,
typesupport,
topic_c_string.as_ptr(),
&publisher_options,
)
};
if result == RCL_RET_OK as i32 {
Ok(publisher_handle)
} else {
Err(Error::from_rcl_error(result))
}
}
impl PublisherUntyped {
pub fn publish(&self, msg: serde_json::Value) -> Result<()> {
// upgrade to actual ref. if still alive
let publisher = self
.handle
.upgrade()
.ok_or(Error::RCL_RET_PUBLISHER_INVALID)?;
let mut native_msg = WrappedNativeMsgUntyped::new_from(&self.type_)?;
native_msg.from_json(msg)?;
let result = unsafe {
rcl_publish(
publisher.as_ref(),
native_msg.void_ptr(),
std::ptr::null_mut(),
)
};
if result == RCL_RET_OK as i32 {
Ok(())
} else {
eprintln!("coult not publish {}", result);
Err(Error::from_rcl_error(result))
}
}
}
impl<T: 'static> Publisher<T>
where
T: WrappedTypesupport,
{
pub fn publish(&self, msg: &T) -> Result<()>
where
T: WrappedTypesupport,
{
// upgrade to actual ref. if still alive
let publisher = self
.handle
.upgrade()
.ok_or(Error::RCL_RET_PUBLISHER_INVALID)?;
let native_msg: WrappedNativeMsg<T> = WrappedNativeMsg::<T>::from(msg);
let result = unsafe {
rcl_publish(
publisher.as_ref(),
native_msg.void_ptr(),
std::ptr::null_mut(),
)
};
if result == RCL_RET_OK as i32 {
Ok(())
} else {
eprintln!("coult not publish {}", result);
Err(Error::from_rcl_error(result))
}
}
pub fn publish_native(&self, msg: &WrappedNativeMsg<T>) -> Result<()>
where
T: WrappedTypesupport,
{
// upgrade to actual ref. if still alive
let publisher = self
.handle
.upgrade()
.ok_or(Error::RCL_RET_PUBLISHER_INVALID)?;
let result =
unsafe { rcl_publish(publisher.as_ref(), msg.void_ptr(), std::ptr::null_mut()) };
if result == RCL_RET_OK as i32 {
Ok(())
} else {
eprintln!("could not publish native {}", result);
Err(Error::from_rcl_error(result))
}
}
}

129
src/services.rs Normal file
View File

@ -0,0 +1,129 @@
use super::*;
/// Encapsulates a service request. In contrast to having a simply callback from
/// Request -> Response types that is called synchronously, the service request
/// can be moved around and completed asynchronously.
#[derive(Clone)]
pub struct ServiceRequest<T>
where
T: WrappedServiceTypeSupport,
{
pub message: T::Request,
request_id: rmw_request_id_t,
service: Weak<Mutex<dyn Service_>>,
}
unsafe impl<T> Send for ServiceRequest<T> where T: WrappedServiceTypeSupport {}
impl<T> ServiceRequest<T>
where
T: 'static + WrappedServiceTypeSupport,
{
/// Complete the service request, consuming the request in the process.
pub fn respond(self, msg: T::Response) -> Result<()> {
let service = self
.service
.upgrade()
.ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
let mut service = service.lock().unwrap();
let native_msg = WrappedNativeMsg::<T::Response>::from(&msg);
service.send_response(self.request_id, Box::new(native_msg))
}
}
pub trait Service_ {
fn handle(&self) -> &rcl_service_t;
fn send_response(&mut self, request_id: rmw_request_id_t, msg: Box<dyn VoidPtr>) -> Result<()>;
fn handle_request(&mut self, service: Arc<Mutex<dyn Service_>>) -> ();
fn destroy(&mut self, node: &mut rcl_node_t) -> ();
}
pub struct TypedService<T>
where
T: WrappedServiceTypeSupport,
{
pub rcl_handle: rcl_service_t,
pub sender: mpsc::Sender<ServiceRequest<T>>,
pub outstanding_requests: Vec<oneshot::Receiver<(rmw_request_id_t, T::Response)>>,
}
impl<T: 'static> Service_ for TypedService<T>
where
T: WrappedServiceTypeSupport,
{
fn handle(&self) -> &rcl_service_t {
&self.rcl_handle
}
fn send_response(
&mut self,
mut request_id: rmw_request_id_t,
mut msg: Box<dyn VoidPtr>,
) -> Result<()> {
let res =
unsafe { rcl_send_response(&self.rcl_handle, &mut request_id, msg.void_ptr_mut()) };
if res == RCL_RET_OK as i32 {
Ok(())
} else {
Err(Error::from_rcl_error(res))
}
}
fn handle_request(&mut self, service: Arc<Mutex<dyn Service_>>) -> () {
let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
let mut request_msg = WrappedNativeMsg::<T::Request>::new();
let ret = unsafe {
rcl_take_request(
&self.rcl_handle,
request_id.as_mut_ptr(),
request_msg.void_ptr_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let request_id = unsafe { request_id.assume_init() };
let request_msg = T::Request::from_native(&request_msg);
let request = ServiceRequest::<T> {
message: request_msg,
request_id,
service: Arc::downgrade(&service),
};
match self.sender.try_send(request) {
Err(e) => eprintln!("warning: could not send service request ({})", e),
_ => (),
}
} // TODO handle failure.
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_service_fini(&mut self.rcl_handle, node);
}
}
}
pub fn create_service_helper(
node: &mut rcl_node_t,
service_name: &str,
service_ts: *const rosidl_service_type_support_t,
) -> Result<rcl_service_t> {
let mut service_handle = unsafe { rcl_get_zero_initialized_service() };
let service_name_c_string =
CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
let result = unsafe {
let service_options = rcl_service_get_default_options();
rcl_service_init(
&mut service_handle,
node,
service_ts,
service_name_c_string.as_ptr(),
&service_options,
)
};
if result == RCL_RET_OK as i32 {
Ok(service_handle)
} else {
Err(Error::from_rcl_error(result))
}
}

157
src/subscribers.rs Normal file
View File

@ -0,0 +1,157 @@
use super::*;
pub trait Subscriber_ {
fn handle(&self) -> &rcl_subscription_t;
fn handle_incoming(&mut self) -> ();
fn destroy(&mut self, node: &mut rcl_node_t) -> ();
}
pub struct TypedSubscriber<T>
where
T: WrappedTypesupport,
{
pub rcl_handle: rcl_subscription_t,
pub sender: mpsc::Sender<T>,
}
pub struct NativeSubscriber<T>
where
T: WrappedTypesupport,
{
pub rcl_handle: rcl_subscription_t,
pub sender: mpsc::Sender<WrappedNativeMsg<T>>,
}
pub struct UntypedSubscriber {
pub rcl_handle: rcl_subscription_t,
pub topic_type: String,
pub sender: mpsc::Sender<Result<serde_json::Value>>,
}
impl<T: 'static> Subscriber_ for TypedSubscriber<T>
where
T: WrappedTypesupport,
{
fn handle(&self) -> &rcl_subscription_t {
&self.rcl_handle
}
fn handle_incoming(&mut self) -> () {
let mut msg_info = rmw_message_info_t::default(); // we dont care for now
let mut msg = WrappedNativeMsg::<T>::new();
let ret = unsafe {
rcl_take(
&self.rcl_handle,
msg.void_ptr_mut(),
&mut msg_info,
std::ptr::null_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let msg = T::from_native(&msg);
match self.sender.try_send(msg) {
Err(e) => println!("error {:?}", e),
_ => (),
}
}
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_subscription_fini(&mut self.rcl_handle, node);
}
}
}
impl<T: 'static> Subscriber_ for NativeSubscriber<T>
where
T: WrappedTypesupport,
{
fn handle(&self) -> &rcl_subscription_t {
&self.rcl_handle
}
fn handle_incoming(&mut self) -> () {
let mut msg_info = rmw_message_info_t::default(); // we dont care for now
let mut msg = WrappedNativeMsg::<T>::new();
let ret = unsafe {
rcl_take(
&self.rcl_handle,
msg.void_ptr_mut(),
&mut msg_info,
std::ptr::null_mut(),
)
};
if ret == RCL_RET_OK as i32 {
match self.sender.try_send(msg) {
Err(e) => println!("error {:?}", e),
_ => (),
}
}
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_subscription_fini(&mut self.rcl_handle, node);
}
}
}
impl Subscriber_ for UntypedSubscriber {
fn handle(&self) -> &rcl_subscription_t {
&self.rcl_handle
}
fn handle_incoming(&mut self) -> () {
let mut msg_info = rmw_message_info_t::default(); // we dont care for now
let mut msg = WrappedNativeMsgUntyped::new_from(&self.topic_type)
.expect(&format!("no typesupport for {}", self.topic_type));
let ret = unsafe {
rcl_take(
&self.rcl_handle,
msg.void_ptr_mut(),
&mut msg_info,
std::ptr::null_mut(),
)
};
if ret == RCL_RET_OK as i32 {
let json = msg.to_json();
match self.sender.try_send(json) {
Err(e) => println!("error {:?}", e),
_ => (),
}
}
}
fn destroy(&mut self, node: &mut rcl_node_t) {
unsafe {
rcl_subscription_fini(&mut self.rcl_handle, node);
}
}
}
pub fn create_subscription_helper(
node: &mut rcl_node_t,
topic: &str,
ts: *const rosidl_message_type_support_t,
) -> Result<rcl_subscription_t> {
let mut subscription_handle = unsafe { rcl_get_zero_initialized_subscription() };
let topic_c_string = CString::new(topic).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
let result = unsafe {
let mut subscription_options = rcl_subscription_get_default_options();
subscription_options.qos = rmw_qos_profile_t::default();
rcl_subscription_init(
&mut subscription_handle,
node,
ts,
topic_c_string.as_ptr(),
&subscription_options,
)
};
if result == RCL_RET_OK as i32 {
Ok(subscription_handle)
} else {
Err(Error::from_rcl_error(result))
}
}