Cleanup package names and imports

This commit is contained in:
Martin Dahl 2021-09-02 11:50:07 +02:00
parent fe557e1bcb
commit 1e0a368d33
41 changed files with 267 additions and 211 deletions

View File

@ -1,7 +1,7 @@
[package] [package]
name = "r2r" name = "r2r"
version = "0.5.1" version = "0.5.2"
authors = ["Martin Dahl <martin.dahl@gmail.com>"] authors = ["Martin Dahl <martin.dahl@gmail.com>"]
description = "Easy to use, runtime-agnostic, async rust bindings for ROS2." description = "Easy to use, runtime-agnostic, async rust bindings for ROS2."
license = "MIT OR Apache-2.0" license = "MIT OR Apache-2.0"
@ -19,10 +19,10 @@ serde = { version = "1.0.123", features = ["derive"] }
serde_json = "1.0.62" serde_json = "1.0.62"
thiserror = "1.0" thiserror = "1.0"
lazy_static = "1.4.0" lazy_static = "1.4.0"
common = { path = "common", version = "0.1.0" } r2r_common = { path = "r2r_common", version = "0.2.0" }
rcl = { path = "rcl", version = "0.1.0" } r2r_rcl = { path = "r2r_rcl", version = "0.2.0" }
msg_gen = { path = "msg_gen", version = "0.1.0" } r2r_msg_gen = { path = "r2r_msg_gen", version = "0.2.0" }
actions = { path = "actions", version = "0.1.0" } r2r_actions = { path = "r2r_actions", version = "0.2.0" }
uuid = { version = "0.8", features = ["serde", "v4"] } uuid = { version = "0.8", features = ["serde", "v4"] }
retain_mut = "0.1.3" retain_mut = "0.1.3"
futures = "0.3.15" futures = "0.3.15"
@ -34,7 +34,7 @@ tokio = { version = "1", features = ["full"] }
rand = "0.8.4" rand = "0.8.4"
[build-dependencies] [build-dependencies]
common = { path = "common", version = "0.1.0" } r2r_common = { path = "r2r_common", version = "0.2.0" }
msg_gen = { path = "msg_gen", version = "0.1.0" } r2r_msg_gen = { path = "r2r_msg_gen", version = "0.2.0" }
[workspace] [workspace]

View File

@ -5,11 +5,11 @@ Easy to use bindings for ROS2 that do *not* require hooking in to the ROS2 build
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/>. 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/>.
This library differ a bit in style from rclpy and rclcpp as it eliminates all synchronous callbacks in favor of rust futures and streams. Coupled with the rust await syntax, this makes it very pleasant to work with ROS services and actions. The library purposefully does not chose an async runtime -- this means that the user needs to take care of any task spawning. This also limits the API to what futures-rs provides. This library differ a bit in style from rclpy and rclcpp as it eliminates all synchronous callbacks in favor of rust futures and streams. Coupled with the rust await syntax, this makes it very pleasant to work with ROS services and actions, even in a single threaded setup (see service.rs example). The library purposefully does not chose an async runtime -- this means that the user needs to take care of any task spawning. This also limits the API to what futures-rs provides.
Manual is available on github pages <https://sequenceplanner.github.io/r2r/> (documention is lacking though). 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. As of now I have no intention of wrapping all of the RCL just for the sake of completeness. These bindings are being written organically when things are needed by me and others so please be aware that the API will change. As of now I have no intention of wrapping all of the RCL just for the sake of completeness. However, if you need some specific functionality, feel free to open an issue or a PR!
How to use How to use
-------------------- --------------------

View File

@ -1,16 +0,0 @@
[package]
name = "actions"
version = "0.1.0"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
edition = "2018"
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
[dependencies]
rcl = { path = "../rcl", version = "0.1.0" }
msg_gen = { path = "../msg_gen", version = "0.1.0" }
[build-dependencies]
bindgen = "0.57.0"
itertools = "0.10.0"
common = { path = "../common", version = "0.1.0" }

View File

@ -1,12 +1,12 @@
use common; use r2r_common;
use msg_gen; use r2r_msg_gen;
use std::env; use std::env;
use std::fs::File; use std::fs::File;
use std::io::Write; use std::io::Write;
use std::path::{Path, PathBuf}; use std::path::{Path, PathBuf};
fn main() { fn main() {
common::print_cargo_watches(); r2r_common::print_cargo_watches();
let msg_list = if let Some(cmake_includes) = env::var("CMAKE_INCLUDE_DIRS").ok() { let msg_list = if let Some(cmake_includes) = env::var("CMAKE_INCLUDE_DIRS").ok() {
let packages = cmake_includes let packages = cmake_includes
@ -15,8 +15,8 @@ fn main() {
.collect::<Vec<_>>(); .collect::<Vec<_>>();
let deps = env::var("CMAKE_IDL_PACKAGES").unwrap_or(String::default()); let deps = env::var("CMAKE_IDL_PACKAGES").unwrap_or(String::default());
let deps = deps.split(":").collect::<Vec<_>>(); let deps = deps.split(":").collect::<Vec<_>>();
let msgs = common::get_ros_msgs(&packages); let msgs = r2r_common::get_ros_msgs(&packages);
common::parse_msgs(&msgs) r2r_common::parse_msgs(&msgs)
.into_iter() .into_iter()
.filter(|msg| deps.contains(&msg.module.as_str())) .filter(|msg| deps.contains(&msg.module.as_str()))
.collect::<Vec<_>>() .collect::<Vec<_>>()
@ -26,10 +26,10 @@ fn main() {
.split(":") .split(":")
.map(|i| Path::new(i)) .map(|i| Path::new(i))
.collect::<Vec<_>>(); .collect::<Vec<_>>();
let msgs = common::get_ros_msgs(&paths); let msgs = r2r_common::get_ros_msgs(&paths);
common::parse_msgs(&msgs) r2r_common::parse_msgs(&msgs)
}; };
let msgs = common::as_map(&msg_list); let msgs = r2r_common::as_map(&msg_list);
let mut modules = String::new(); let mut modules = String::new();
@ -50,11 +50,11 @@ fn main() {
codegen.push_str(&format!(" pub mod {} {{\n", msg)); codegen.push_str(&format!(" pub mod {} {{\n", msg));
codegen.push_str(" use super::super::super::*;\n"); codegen.push_str(" use super::super::super::*;\n");
codegen.push_str(&msg_gen::generate_rust_action(module, prefix, msg)); codegen.push_str(&r2r_msg_gen::generate_rust_action(module, prefix, msg));
for s in &["Goal", "Result", "Feedback"] { for s in &["Goal", "Result", "Feedback"] {
let msgname = format!("{}_{}", msg, s); let msgname = format!("{}_{}", msg, s);
codegen.push_str(&msg_gen::generate_rust_msg(module, prefix, &msgname)); codegen.push_str(&r2r_msg_gen::generate_rust_msg(module, prefix, &msgname));
println!("cargo:rustc-cfg=r2r__{}__{}__{}", module, prefix, msg); println!("cargo:rustc-cfg=r2r__{}__{}__{}", module, prefix, msg);
} }
@ -65,18 +65,18 @@ fn main() {
codegen.push_str(" use super::super::super::super::*;\n"); codegen.push_str(" use super::super::super::super::*;\n");
let srvname = format!("{}_{}", msg, srv); let srvname = format!("{}_{}", msg, srv);
codegen.push_str(&msg_gen::generate_rust_service(module, prefix, &srvname)); codegen.push_str(&r2r_msg_gen::generate_rust_service(module, prefix, &srvname));
for s in &["Request", "Response"] { for s in &["Request", "Response"] {
let msgname = format!("{}_{}_{}", msg, srv, s); let msgname = format!("{}_{}_{}", msg, srv, s);
codegen.push_str(&msg_gen::generate_rust_msg(module, prefix, &msgname)); codegen.push_str(&r2r_msg_gen::generate_rust_msg(module, prefix, &msgname));
} }
codegen.push_str(" }\n"); codegen.push_str(" }\n");
} }
// also "internal" feedback message type that wraps the feedback type with a uuid // also "internal" feedback message type that wraps the feedback type with a uuid
let feedback_msgname = format!("{}_FeedbackMessage", msg); let feedback_msgname = format!("{}_FeedbackMessage", msg);
codegen.push_str(&msg_gen::generate_rust_msg( codegen.push_str(&r2r_msg_gen::generate_rust_msg(
module, module,
prefix, prefix,
&feedback_msgname, &feedback_msgname,
@ -90,11 +90,11 @@ fn main() {
codegen.push_str(&format!(" pub mod {} {{\n", msg)); codegen.push_str(&format!(" pub mod {} {{\n", msg));
codegen.push_str(" use super::super::super::*;\n"); codegen.push_str(" use super::super::super::*;\n");
codegen.push_str(&msg_gen::generate_rust_service(module, prefix, msg)); codegen.push_str(&r2r_msg_gen::generate_rust_service(module, prefix, msg));
for s in &["Request", "Response"] { for s in &["Request", "Response"] {
let msgname = format!("{}_{}", msg, s); let msgname = format!("{}_{}", msg, s);
codegen.push_str(&msg_gen::generate_rust_msg(module, prefix, &msgname)); codegen.push_str(&r2r_msg_gen::generate_rust_msg(module, prefix, &msgname));
println!("cargo:rustc-cfg=r2r__{}__{}__{}", module, prefix, msg); println!("cargo:rustc-cfg=r2r__{}__{}__{}", module, prefix, msg);
} }
codegen.push_str(" }\n"); codegen.push_str(" }\n");
@ -102,7 +102,7 @@ fn main() {
} else if prefix == &"msg" { } else if prefix == &"msg" {
codegen.push_str(" use super::super::*;\n"); codegen.push_str(" use super::super::*;\n");
for msg in msgs { for msg in msgs {
codegen.push_str(&msg_gen::generate_rust_msg(module, prefix, msg)); codegen.push_str(&r2r_msg_gen::generate_rust_msg(module, prefix, msg));
println!("cargo:rustc-cfg=r2r__{}__{}__{}", module, prefix, msg); println!("cargo:rustc-cfg=r2r__{}__{}__{}", module, prefix, msg);
} }
} else { } else {
@ -117,9 +117,9 @@ fn main() {
write!(f, "{}", codegen).unwrap(); write!(f, "{}", codegen).unwrap();
} }
let untyped_helper = msg_gen::generate_untyped_helper(&msg_list); let untyped_helper = r2r_msg_gen::generate_untyped_helper(&msg_list);
let untyped_service_helper = msg_gen::generate_untyped_service_helper(&msg_list); let untyped_service_helper = r2r_msg_gen::generate_untyped_service_helper(&msg_list);
let untyped_action_helper = msg_gen::generate_untyped_action_helper(&msg_list); let untyped_action_helper = r2r_msg_gen::generate_untyped_action_helper(&msg_list);
let out_path = PathBuf::from(env::var("OUT_DIR").unwrap()); let out_path = PathBuf::from(env::var("OUT_DIR").unwrap());
let msgs_fn = out_path.join("_r2r_generated_msgs.rs"); let msgs_fn = out_path.join("_r2r_generated_msgs.rs");

View File

@ -1,8 +0,0 @@
[package]
name = "common"
version = "0.1.0"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
description = "Minimal ros2 bindings."
license = "Apache-2.0/MIT"
edition = "2018"

View File

@ -11,13 +11,13 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
let mut clock = r2r::Clock::create(r2r::ClockType::SystemTime)?; let mut clock = r2r::Clock::create(r2r::ClockType::SystemTime)?;
let now = clock.get_now()?; let now = clock.get_now()?;
let time = r2r::Clock::to_builtin_time(&now); let time = r2r::Clock::to_builtin_time(&now);
println!("rostime: {:?}", time); println!("systemtime: {:?}", time);
} }
{ {
let mut clock = r2r::Clock::create(r2r::ClockType::SteadyTime)?; let mut clock = r2r::Clock::create(r2r::ClockType::SteadyTime)?;
let now = clock.get_now()?; let now = clock.get_now()?;
let time = r2r::Clock::to_builtin_time(&now); let time = r2r::Clock::to_builtin_time(&now);
println!("rostime: {:?}", time); println!("steadytime: {:?}", time);
} }
Ok(()) Ok(())
} }

View File

@ -1,16 +0,0 @@
[package]
name = "msg_gen"
version = "0.1.0"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
edition = "2018"
[dependencies]
lazy_static = "1.4.0"
rcl = { path = "../rcl", version = "0.1.0" }
common = { path = "../common", version = "0.1.0" }
[build-dependencies]
bindgen = "0.57.0"
rcl = { path = "../rcl", version = "0.1.0" }
common = { path = "../common", version = "0.1.0" }
heck = "0.3.2"

18
r2r_actions/Cargo.toml Normal file
View File

@ -0,0 +1,18 @@
[package]
name = "r2r_actions"
version = "0.2.0"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
edition = "2018"
readme = "README.md"
homepage = "https://github.com/sequenceplanner/r2r"
repository = "https://github.com/sequenceplanner/r2r"
documentation = "https://sequenceplanner.github.io/r2r/"
[dependencies]
r2r_rcl = { path = "../r2r_rcl", version = "0.2.0" }
r2r_msg_gen = { path = "../r2r_msg_gen", version = "0.2.0" }
[build-dependencies]
bindgen = "0.57.0"
itertools = "0.10.0"
r2r_common = { path = "../r2r_common", version = "0.2.0" }

1
r2r_actions/README.md Normal file
View File

@ -0,0 +1 @@
Internal dependency of r2r <https://github.com/sequenceplanner/r2r>.

View File

@ -5,5 +5,5 @@
#![allow(dead_code)] #![allow(dead_code)]
include!(concat!(env!("OUT_DIR"), "/action_bindings.rs")); include!(concat!(env!("OUT_DIR"), "/action_bindings.rs"));
use msg_gen::*; use r2r_msg_gen::*;
use rcl::*; use r2r_rcl::*;

11
r2r_common/Cargo.toml Normal file
View File

@ -0,0 +1,11 @@
[package]
name = "r2r_common"
version = "0.2.0"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
description = "Minimal ros2 bindings."
license = "Apache-2.0/MIT"
edition = "2018"
readme = "README.md"
homepage = "https://github.com/sequenceplanner/r2r"
repository = "https://github.com/sequenceplanner/r2r"
documentation = "https://sequenceplanner.github.io/r2r/"

1
r2r_common/README.md Normal file
View File

@ -0,0 +1 @@
Internal dependency of r2r <https://github.com/sequenceplanner/r2r>.

16
r2r_msg_gen/Cargo.toml Normal file
View File

@ -0,0 +1,16 @@
[package]
name = "r2r_msg_gen"
version = "0.2.0"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
edition = "2018"
[dependencies]
lazy_static = "1.4.0"
r2r_rcl = { path = "../r2r_rcl", version = "0.2.0" }
r2r_common = { path = "../r2r_common", version = "0.2.0" }
[build-dependencies]
bindgen = "0.57.0"
r2r_rcl = { path = "../r2r_rcl", version = "0.2.0" }
r2r_common = { path = "../r2r_common", version = "0.2.0" }
heck = "0.3.2"

1
r2r_msg_gen/README.md Normal file
View File

@ -0,0 +1 @@
Internal dependency of r2r <https://github.com/sequenceplanner/r2r>.

View File

@ -1,12 +1,12 @@
use bindgen; use bindgen;
use common; use r2r_common;
use std::env; use std::env;
use std::fs::File; use std::fs::File;
use std::io::Write; use std::io::Write;
use std::path::{Path, PathBuf}; use std::path::{Path, PathBuf};
fn main() { fn main() {
common::print_cargo_watches(); r2r_common::print_cargo_watches();
let mut builder = bindgen::Builder::default(); let mut builder = bindgen::Builder::default();
@ -20,8 +20,8 @@ fn main() {
} }
let deps = env::var("CMAKE_IDL_PACKAGES").unwrap_or(String::default()); let deps = env::var("CMAKE_IDL_PACKAGES").unwrap_or(String::default());
let deps = deps.split(":").collect::<Vec<_>>(); let deps = deps.split(":").collect::<Vec<_>>();
let msgs = common::get_ros_msgs(&packages); let msgs = r2r_common::get_ros_msgs(&packages);
common::parse_msgs(&msgs) r2r_common::parse_msgs(&msgs)
.into_iter() .into_iter()
.filter(|msg| deps.contains(&msg.module.as_str())) .filter(|msg| deps.contains(&msg.module.as_str()))
.collect::<Vec<_>>() .collect::<Vec<_>>()
@ -35,11 +35,11 @@ fn main() {
.map(|i| Path::new(i)) .map(|i| Path::new(i))
.collect::<Vec<_>>(); .collect::<Vec<_>>();
let msgs = common::get_ros_msgs(&paths); let msgs = r2r_common::get_ros_msgs(&paths);
common::parse_msgs(&msgs) r2r_common::parse_msgs(&msgs)
}; };
let msg_map = common::as_map(&msg_list); let msg_map = r2r_common::as_map(&msg_list);
for module in msg_map.keys() { for module in msg_map.keys() {
println!( println!(

View File

@ -9,7 +9,7 @@ include!(concat!(env!("OUT_DIR"), "/introspection_functions.rs"));
#[macro_use] #[macro_use]
extern crate lazy_static; extern crate lazy_static;
use rcl::*; use r2r_rcl::*;
use std::collections::HashMap; use std::collections::HashMap;
use std::ffi::CStr; use std::ffi::CStr;
@ -509,7 +509,7 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String {
} }
} }
pub fn generate_untyped_helper(msgs: &Vec<common::RosMsg>) -> String { pub fn generate_untyped_helper(msgs: &Vec<r2r_common::RosMsg>) -> String {
let open = String::from( let open = String::from(
" "
impl WrappedNativeMsgUntyped { impl WrappedNativeMsgUntyped {
@ -551,7 +551,7 @@ impl WrappedNativeMsgUntyped {
format!("{}{}{}", open, lines, close) format!("{}{}{}", open, lines, close)
} }
pub fn generate_untyped_service_helper(msgs: &Vec<common::RosMsg>) -> String { pub fn generate_untyped_service_helper(msgs: &Vec<r2r_common::RosMsg>) -> String {
let open = String::from( let open = String::from(
" "
impl UntypedServiceSupport { impl UntypedServiceSupport {
@ -592,7 +592,7 @@ impl UntypedServiceSupport {
format!("{}{}{}", open, lines, close) format!("{}{}{}", open, lines, close)
} }
pub fn generate_untyped_action_helper(msgs: &Vec<common::RosMsg>) -> String { pub fn generate_untyped_action_helper(msgs: &Vec<r2r_common::RosMsg>) -> String {
let open = String::from( let open = String::from(
" "
impl UntypedActionSupport { impl UntypedActionSupport {

18
r2r_rcl/Cargo.toml Normal file
View File

@ -0,0 +1,18 @@
[package]
name = "r2r_rcl"
version = "0.2.0"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
edition = "2018"
readme = "README.md"
homepage = "https://github.com/sequenceplanner/r2r"
repository = "https://github.com/sequenceplanner/r2r"
documentation = "https://sequenceplanner.github.io/r2r/"
[dependencies]
paste = "1.0.4"
widestring = "0.4.3"
[build-dependencies]
bindgen = "0.57.0"
itertools = "0.10.0"
r2r_common = { path = "../r2r_common", version = "0.2.0" }

1
r2r_rcl/README.md Normal file
View File

@ -0,0 +1 @@
Internal dependency of r2r <https://github.com/sequenceplanner/r2r>.

View File

@ -1,12 +1,12 @@
extern crate bindgen; extern crate bindgen;
use common; use r2r_common;
use itertools::Itertools; use itertools::Itertools;
use std::env; use std::env;
use std::path::{Path, PathBuf}; use std::path::{Path, PathBuf};
fn main() { fn main() {
common::print_cargo_watches(); r2r_common::print_cargo_watches();
let mut builder = bindgen::Builder::default() let mut builder = bindgen::Builder::default()
.header("src/rcl_wrapper.h") .header("src/rcl_wrapper.h")

View File

@ -1,14 +0,0 @@
[package]
name = "rcl"
version = "0.1.0"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
edition = "2018"
[dependencies]
paste = "1.0.4"
widestring = "0.4.3"
[build-dependencies]
bindgen = "0.57.0"
itertools = "0.10.0"
common = { path = "../common", version = "0.1.0" }

View File

@ -7,7 +7,16 @@ use std::sync::{Mutex, Weak};
use std::mem::MaybeUninit; use std::mem::MaybeUninit;
use std::ffi::CString; use std::ffi::CString;
use super::*; use crate::error::*;
use crate::action_common::*;
use crate::msg_types::*;
use crate::msg_types::generated_msgs::{
unique_identifier_msgs,
action_msgs,
builtin_interfaces,
};
use r2r_rcl::*;
use r2r_actions::*;
unsafe impl<T> Send for ActionClient<T> where T: WrappedActionTypeSupport {} unsafe impl<T> Send for ActionClient<T> where T: WrappedActionTypeSupport {}
@ -172,62 +181,6 @@ where
ActionClient { client } ActionClient { client }
} }
/// The status of a goal.
#[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> pub struct WrappedActionClient<T>
where where
T: WrappedActionTypeSupport, T: WrappedActionTypeSupport,
@ -260,13 +213,6 @@ pub trait ActionClient_ {
fn poll_available(&mut self, node: &mut rcl_node_t) -> (); 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> impl<T> WrappedActionClient<T>
where where
T: WrappedActionTypeSupport, T: WrappedActionTypeSupport,
@ -421,7 +367,7 @@ where
if ret == RCL_RET_OK as i32 { if ret == RCL_RET_OK as i32 {
let msg = T::FeedbackMessage::from_native(&feedback_msg); let msg = T::FeedbackMessage::from_native(&feedback_msg);
let (uuid, feedback) = T::destructure_feedback_msg(msg); let (uuid, feedback) = T::destructure_feedback_msg(msg);
let msg_uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(uuid.uuid)); let msg_uuid = uuid_msg_to_uuid(&uuid);
if let Some((_, sender)) = self if let Some((_, sender)) = self
.feedback_senders .feedback_senders
.iter_mut() .iter_mut()
@ -441,8 +387,7 @@ where
if ret == RCL_RET_OK as i32 { if ret == RCL_RET_OK as i32 {
let arr = action_msgs::msg::GoalStatusArray::from_native(&status_array); let arr = action_msgs::msg::GoalStatusArray::from_native(&status_array);
for a in &arr.status_list { for a in &arr.status_list {
let uuid = let uuid = uuid_msg_to_uuid(&a.goal_info.goal_id);
uuid::Uuid::from_bytes(vec_to_uuid_bytes(a.goal_info.goal_id.uuid.clone()));
if !self.result_senders.iter().any(|(suuid, _)| suuid == &uuid) { if !self.result_senders.iter().any(|(suuid, _)| suuid == &uuid) {
continue; continue;
} }

View File

@ -6,8 +6,17 @@ use std::future::Future;
use std::sync::{Mutex, Weak}; use std::sync::{Mutex, Weak};
use std::mem::MaybeUninit; use std::mem::MaybeUninit;
use super::*; use crate::error::*;
use crate::action_common::*;
use crate::msg_types::*;
use crate::action_clients::*;
use crate::msg_types::generated_msgs::{
unique_identifier_msgs,
action_msgs,
builtin_interfaces,
};
use r2r_rcl::*;
use r2r_actions::*;
// //
// TODO: refactor this to separate out shared code between typed action client and this. // TODO: refactor this to separate out shared code between typed action client and this.
// //
@ -320,7 +329,7 @@ impl ActionClient_ for WrappedActionClientUntyped {
if ret == RCL_RET_OK as i32 { if ret == RCL_RET_OK as i32 {
let (uuid, feedback) = let (uuid, feedback) =
(self.action_type_support.destructure_feedback_msg)(feedback_msg); (self.action_type_support.destructure_feedback_msg)(feedback_msg);
let msg_uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(uuid.uuid)); let msg_uuid = uuid_msg_to_uuid(&uuid);
if let Some((_, sender)) = self if let Some((_, sender)) = self
.feedback_senders .feedback_senders
.iter_mut() .iter_mut()
@ -340,8 +349,7 @@ impl ActionClient_ for WrappedActionClientUntyped {
if ret == RCL_RET_OK as i32 { if ret == RCL_RET_OK as i32 {
let arr = action_msgs::msg::GoalStatusArray::from_native(&status_array); let arr = action_msgs::msg::GoalStatusArray::from_native(&status_array);
for a in &arr.status_list { for a in &arr.status_list {
let uuid = let uuid = uuid_msg_to_uuid(&a.goal_info.goal_id);
uuid::Uuid::from_bytes(vec_to_uuid_bytes(a.goal_info.goal_id.uuid.clone()));
if !self.result_senders.iter().any(|(suuid, _)| suuid == &uuid) { if !self.result_senders.iter().any(|(suuid, _)| suuid == &uuid) {
continue; continue;
} }

55
src/action_common.rs Normal file
View File

@ -0,0 +1,55 @@
/// The status of a goal.
#[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)
}
}

View File

@ -8,7 +8,16 @@ use std::ffi::CString;
use std::mem::MaybeUninit; use std::mem::MaybeUninit;
use std::sync::{Arc, Mutex, Weak}; use std::sync::{Arc, Mutex, Weak};
use super::*; use crate::error::*;
use crate::action_common::*;
use crate::msg_types::*;
use crate::msg_types::generated_msgs::{
unique_identifier_msgs,
action_msgs,
builtin_interfaces,
};
use r2r_rcl::*;
use r2r_actions::*;
pub trait ActionServer_ { pub trait ActionServer_ {
fn handle(&self) -> &rcl_action_server_t; fn handle(&self) -> &rcl_action_server_t;
@ -283,9 +292,7 @@ where
} }
response_msg.goals_canceling.retain(|goal_info| { response_msg.goals_canceling.retain(|goal_info| {
let msg_uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes( let msg_uuid = uuid_msg_to_uuid(&goal_info.goal_id);
goal_info.goal_id.uuid.clone(),
));
do_cancel && msg_uuid == uuid do_cancel && msg_uuid == uuid
}); });
} }
@ -353,7 +360,7 @@ where
} }
let msg = <<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request>::from_native(&request_msg); 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_msg, goal) = T::destructure_goal_request_msg(msg);
let uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(uuid_msg.uuid.clone())); let uuid = uuid_msg_to_uuid(&uuid_msg);
let (cancel_sender, cancel_receiver) = mpsc::channel::<ActionServerCancelRequest>(10); let (cancel_sender, cancel_receiver) = mpsc::channel::<ActionServerCancelRequest>(10);
self.cancel_senders.insert(uuid.clone(), cancel_sender); self.cancel_senders.insert(uuid.clone(), cancel_sender);
@ -408,8 +415,7 @@ where
.goals_canceling .goals_canceling
.iter() .iter()
.flat_map(|goal_info| { .flat_map(|goal_info| {
let uuid = let uuid = uuid_msg_to_uuid(&goal_info.goal_id);
uuid::Uuid::from_bytes(vec_to_uuid_bytes(goal_info.goal_id.uuid.clone()));
self.cancel_senders self.cancel_senders
.get_mut(&uuid) .get_mut(&uuid)
.and_then(|cancel_sender| { .and_then(|cancel_sender| {
@ -448,7 +454,7 @@ where
return; return;
} }
let gi = action_msgs::msg::GoalInfo::from_native(&goal_info); 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())); let uuid = uuid_msg_to_uuid(&gi.goal_id);
println!("goal expired: {} - {}", uuid, num_expired); println!("goal expired: {} - {}", uuid, num_expired);
// todo // todo
// self.goals.remove(&uuid); // self.goals.remove(&uuid);
@ -539,7 +545,7 @@ where
let goal_exists = let goal_exists =
unsafe { rcl_action_server_goal_exists(&self.rcl_handle, &*goal_info_native) }; 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 uuid = uuid_msg_to_uuid(&goal_info.goal_id);
let response_msg = if !goal_exists { let response_msg = if !goal_exists {
// Goal does not exists // Goal does not exists

View File

@ -7,7 +7,7 @@ use std::sync::{Mutex, Weak};
use crate::msg_types::*; use crate::msg_types::*;
use crate::error::*; use crate::error::*;
use rcl::*; use r2r_rcl::*;
/// ROS service client. /// ROS service client.
/// ///

View File

@ -2,7 +2,9 @@ use std::fmt::Debug;
use std::mem::MaybeUninit; use std::mem::MaybeUninit;
use std::time::Duration; use std::time::Duration;
use super::*; use crate::error::*;
use crate::msg_types::generated_msgs::builtin_interfaces;
use r2r_rcl::*;
/// Different ROS clock types. /// Different ROS clock types.
#[derive(Debug)] #[derive(Debug)]

View File

@ -3,7 +3,9 @@ use std::fmt::Debug;
use std::ops::{Deref, DerefMut}; use std::ops::{Deref, DerefMut};
use std::sync::{Arc, Mutex}; use std::sync::{Arc, Mutex};
use super::*; use crate::error::*;
use crate::log_guard;
use r2r_rcl::*;
/// A ROS context. Needed to create nodes etc. /// A ROS context. Needed to create nodes etc.
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
@ -89,8 +91,8 @@ impl Drop for ContextHandle {
fn drop(&mut self) { fn drop(&mut self) {
// TODO: error handling? atleast probably need rcl_reset_error // TODO: error handling? atleast probably need rcl_reset_error
unsafe { unsafe {
rcl::rcl_shutdown(self.0.as_mut()); rcl_shutdown(self.0.as_mut());
rcl::rcl_context_fini(self.0.as_mut()); rcl_context_fini(self.0.as_mut());
} }
} }
} }

View File

@ -1,6 +1,6 @@
#![allow(non_camel_case_types)] #![allow(non_camel_case_types)]
use actions::*; use r2r_actions::*;
use rcl::*; use r2r_rcl::*;
use thiserror::Error; use thiserror::Error;
/// r2r Result type. /// r2r Result type.

View File

@ -60,7 +60,7 @@
//! } //! }
//! })?; //! })?;
//! //!
//! // Main thread spins ros. //! // Main loop spins ros.
//! loop { //! loop {
//! node.spin_once(std::time::Duration::from_millis(100)); //! node.spin_once(std::time::Duration::from_millis(100));
//! pool.run_until_stalled(); //! pool.run_until_stalled();
@ -73,15 +73,10 @@
// this crate depend on, which seem like bad user experience. // this crate depend on, which seem like bad user experience.
pub extern crate uuid; pub extern crate uuid;
use actions::*;
use rcl::*;
mod error; mod error;
pub use error::{Error, Result}; pub use error::{Error, Result};
mod msg_types; mod msg_types;
use msg_types::*;
pub use msg_types::generated_msgs::*; pub use msg_types::generated_msgs::*;
pub use msg_types::WrappedNativeMsg as NativeMsg; pub use msg_types::WrappedNativeMsg as NativeMsg;
@ -89,30 +84,26 @@ mod utils;
pub use utils::*; pub use utils::*;
mod subscribers; mod subscribers;
use subscribers::*;
mod publishers; mod publishers;
use publishers::*;
pub use publishers::{Publisher, PublisherUntyped}; pub use publishers::{Publisher, PublisherUntyped};
mod services; mod services;
pub use services::ServiceRequest; pub use services::ServiceRequest;
use services::*;
mod clients; mod clients;
use clients::*;
pub use clients::{Client, ClientUntyped}; pub use clients::{Client, ClientUntyped};
mod action_common;
pub use action_common::GoalStatus;
mod action_clients; mod action_clients;
use action_clients::*; pub use action_clients::{ActionClient, ActionClientGoal};
pub use action_clients::{ActionClient, ActionClientGoal, GoalStatus};
mod action_clients_untyped; mod action_clients_untyped;
use action_clients_untyped::*;
pub use action_clients_untyped::{ActionClientGoalUntyped, ActionClientUntyped}; pub use action_clients_untyped::{ActionClientGoalUntyped, ActionClientUntyped};
mod action_servers; mod action_servers;
use action_servers::*;
pub use action_servers::{ActionServerCancelRequest, ActionServerGoal, ActionServerGoalRequest}; pub use action_servers::{ActionServerCancelRequest, ActionServerGoal, ActionServerGoalRequest};
mod context; mod context;

View File

@ -1,11 +1,12 @@
use crate::error::*; use crate::error::*;
use msg_gen::*; use r2r_msg_gen::*;
use rcl::{ use r2r_rcl::{
rosidl_action_type_support_t, rosidl_message_type_support_t, rosidl_service_type_support_t, rosidl_action_type_support_t, rosidl_message_type_support_t, rosidl_service_type_support_t,
}; };
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
use std::fmt::Debug; use std::fmt::Debug;
use std::ops::{Deref, DerefMut}; use std::ops::{Deref, DerefMut};
use std::convert::TryInto;
pub mod generated_msgs { pub mod generated_msgs {
use super::*; use super::*;
@ -23,6 +24,18 @@ pub mod generated_msgs {
use generated_msgs::{builtin_interfaces, unique_identifier_msgs}; use generated_msgs::{builtin_interfaces, unique_identifier_msgs};
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())
})
}
/// TODO: maybe expose this somewhere.
pub(crate) fn uuid_msg_to_uuid(msg: &unique_identifier_msgs::msg::UUID) -> uuid::Uuid {
let bytes = vec_to_uuid_bytes(msg.uuid.clone());
uuid::Uuid::from_bytes(bytes)
}
pub trait WrappedTypesupport: pub trait WrappedTypesupport:
Serialize + serde::de::DeserializeOwned + Default + Debug + Clone Serialize + serde::de::DeserializeOwned + Default + Debug + Clone
{ {
@ -396,7 +409,7 @@ where
mod tests { mod tests {
use super::generated_msgs::*; use super::generated_msgs::*;
use super::*; use super::*;
use rcl::*; use r2r_rcl::*;
#[test] #[test]
fn test_ros_str() -> () { fn test_ros_str() -> () {

View File

@ -10,7 +10,22 @@ use std::mem::MaybeUninit;
use std::sync::{Arc, Mutex}; use std::sync::{Arc, Mutex};
use std::time::Duration; use std::time::Duration;
use super::*; use r2r_rcl::*;
use r2r_actions::*;
use crate::error::*;
use crate::msg_types::*;
use crate::msg_types::generated_msgs::rcl_interfaces;
use crate::subscribers::*;
use crate::publishers::*;
use crate::services::*;
use crate::clients::*;
use crate::action_clients::*;
use crate::action_clients_untyped::*;
use crate::action_servers::*;
use crate::context::*;
use crate::parameters::*;
use crate::clocks::*;
/// A ROS Node. /// A ROS Node.
/// ///

View File

@ -1,6 +1,7 @@
use std::ffi::CStr; use std::ffi::CStr;
use super::*; use crate::msg_types::generated_msgs::rcl_interfaces;
use r2r_rcl::*;
/// ROS parameter value. /// ROS parameter value.
#[derive(Debug, PartialEq, Clone)] #[derive(Debug, PartialEq, Clone)]

View File

@ -3,8 +3,9 @@ use std::fmt::Debug;
use std::sync::Weak; use std::sync::Weak;
use std::marker::PhantomData; use std::marker::PhantomData;
use super::*; use crate::msg_types::*;
use crate::error::*;
use r2r_rcl::*;
// The publish function is thread safe. ROS2 docs state: // The publish function is thread safe. ROS2 docs state:
// ============= // =============

View File

@ -3,7 +3,9 @@ use std::ffi::CString;
use std::sync::{Arc, Mutex, Weak}; use std::sync::{Arc, Mutex, Weak};
use std::mem::MaybeUninit; use std::mem::MaybeUninit;
use super::*; use crate::msg_types::*;
use crate::error::*;
use r2r_rcl::*;
/// Encapsulates a service request. /// Encapsulates a service request.
/// ///

View File

@ -1,7 +1,9 @@
use futures::channel::mpsc; use futures::channel::mpsc;
use std::ffi::CString; use std::ffi::CString;
use super::*; use crate::msg_types::*;
use crate::error::*;
use r2r_rcl::*;
pub trait Subscriber_ { pub trait Subscriber_ {
fn handle(&self) -> &rcl_subscription_t; fn handle(&self) -> &rcl_subscription_t;

View File

@ -1,4 +1,4 @@
use rcl::*; use r2r_rcl::*;
use std::ffi::CString; use std::ffi::CString;
use std::sync::{Mutex, MutexGuard}; use std::sync::{Mutex, MutexGuard};