Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update ros2-client and rustdds dependencies to latest fork version #397

Merged
merged 5 commits into from
Jan 4, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/ci.yml
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,7 @@ jobs:
ros2-bridge-examples:
name: "ROS2 Bridge Examples"
runs-on: ubuntu-latest
timeout-minutes: 30
timeout-minutes: 45
steps:
- uses: actions/checkout@v3
- uses: r7kamura/[email protected]
Expand Down
85 changes: 67 additions & 18 deletions Cargo.lock

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

16 changes: 12 additions & 4 deletions examples/python-ros2-dataflow/random_turtle.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,12 @@

print("looping", flush=True)

# take track of minimum and maximum coordinates of turtle
min_x = 1000
max_x = 0
min_y = 1000
max_y = 0

for i in range(500):
event = dora_node.next()
if event is None:
Expand All @@ -55,8 +61,10 @@
# ROS2 Event
elif event_kind == "external":
pose = event.inner()[0].as_py()
if i == CHECK_TICK:
assert (
pose["x"] != 5.544444561004639
), "turtle should not be at initial x axis"
min_x = min([min_x, pose["x"]])
max_x = max([max_x, pose["x"]])
min_y = min([min_y, pose["y"]])
max_y = max([max_y, pose["y"]])
dora_node.send_output("turtle_pose", event.inner())

assert max_x - min_x > 1 or max_y - min_y > 1, "no turtle movement"
18 changes: 10 additions & 8 deletions examples/rust-ros2-dataflow/node/src/main.rs
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ use dora_ros2_bridge::{
rustdds::{self, policy},
turtlesim::msg::Pose,
};
use eyre::Context;
use eyre::{eyre, Context};

fn main() -> eyre::Result<()> {
let mut ros_node = init_ros_node()?;
Expand Down Expand Up @@ -79,8 +79,8 @@ fn init_ros_node() -> eyre::Result<ros2_client::Node> {

ros_context
.new_node(
"turtle_teleop", // name
"/ros2_demo", // namespace
ros2_client::NodeName::new("/ros2_demo", "turtle_teleop")
.map_err(|e| eyre!("failed to create ROS2 node name: {e}"))?,
NodeOptions::new().enable_rosout(true),
)
.context("failed to create ros2 node")
Expand All @@ -93,7 +93,7 @@ fn create_vel_publisher(
rustdds::QosPolicyBuilder::new()
.durability(policy::Durability::Volatile)
.liveliness(policy::Liveliness::Automatic {
lease_duration: ros2::Duration::DURATION_INFINITE,
lease_duration: ros2::Duration::INFINITE,
})
.reliability(policy::Reliability::Reliable {
max_blocking_time: ros2::Duration::from_millis(100),
Expand All @@ -104,8 +104,9 @@ fn create_vel_publisher(

let turtle_cmd_vel_topic = ros_node
.create_topic(
"/turtle1/cmd_vel",
String::from("geometry_msgs::msg::dds_::Twist_"),
&ros2_client::Name::new("/turtle1", "cmd_vel")
.map_err(|e| eyre!("failed to create ROS2 name: {e}"))?,
ros2_client::MessageTypeName::new("geometry_msgs", "Twist"),
&topic_qos,
)
.context("failed to create topic")?;
Expand All @@ -122,8 +123,9 @@ fn create_pose_reader(
) -> eyre::Result<ros2_client::Subscription<Pose>> {
let turtle_pose_topic = ros_node
.create_topic(
"/turtle1/pose",
String::from("turtlesim::msg::dds_::Pose_"),
&ros2_client::Name::new("/turtle1", "pose")
.map_err(|e| eyre!("failed to create ROS2 name: {e}"))?,
ros2_client::MessageTypeName::new("turtlesim", "Pose"),
&Default::default(),
)
.context("failed to create topic")?;
Expand Down
4 changes: 2 additions & 2 deletions libraries/extensions/ros2-bridge/Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@ dora-ros2-bridge-msg-gen-macro = { path = "msg-gen-macro", optional = true }
serde = { version = "1.0.164", features = ["derive"] }
serde-big-array = "0.5.1"
widestring = "1.0.2"
ros2-client = { git = "https://github.com/dora-rs/ros2-client.git", branch = "deserialize-seed" }
rustdds = { git = "https://github.com/dora-rs/RustDDS.git", branch = "deserialize-seed" }
ros2-client = { git = "https://github.com/dora-rs/ros2-client.git", branch = "deserialize-seed-2" }
rustdds = { git = "https://github.com/dora-rs/RustDDS.git", branch = "deserialize-seed-2" }
eyre = { version = "0.6.8", optional = true }
tokio = { version = "1.29.1", features = ["full"], optional = true }
dora-daemon = { path = "../../../binaries/daemon", optional = true }
Expand Down
16 changes: 11 additions & 5 deletions libraries/extensions/ros2-bridge/python/src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,10 @@ impl Ros2Context {
namespace: &str,
options: Ros2NodeOptions,
) -> eyre::Result<Ros2Node> {
let name = ros2_client::NodeName::new(namespace, name)
.map_err(|err| eyre!("invalid node name: {err}"))?;
Ok(Ros2Node {
node: self.context.new_node(name, namespace, options.into())?,
node: self.context.new_node(name, options.into())?,
messages: self.messages.clone(),
})
}
Expand All @@ -103,12 +105,16 @@ impl Ros2Node {
message_type
)
})?;
let encoded_type_name = format!("{namespace_name}::msg::dds_::{message_name}_");
let message_type_name = ros2_client::MessageTypeName::new(namespace_name, message_name);
let topic_name = ros2_client::Name::parse(name)
.map_err(|err| eyre!("failed to parse ROS2 topic name: {err}"))?;
let topic = self
.node
.create_topic(name, encoded_type_name, &qos.into())?;
let type_info = for_message(&self.messages, namespace_name, message_name)
.context("failed to determine type info for message")?;
.create_topic(&topic_name, message_type_name, &qos.into())?;
let type_info =
for_message(&self.messages, namespace_name, message_name).with_context(|| {
format!("failed to determine type info for message {namespace_name}/{message_name}")
})?;

Ok(Ros2Topic { topic, type_info })
}
Expand Down
2 changes: 1 addition & 1 deletion libraries/extensions/ros2-bridge/python/src/qos.rs
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ pub enum Ros2Liveliness {
impl Ros2Liveliness {
fn convert(self, lease_duration: f64) -> policy::Liveliness {
let lease_duration = if lease_duration.is_infinite() {
rustdds::Duration::DURATION_INFINITE
rustdds::Duration::INFINITE
} else {
rustdds::Duration::from_frac_seconds(lease_duration)
};
Expand Down
Loading