From 728fd0d366f9bc65da9c318d64a0c5e8c385cd89 Mon Sep 17 00:00:00 2001 From: yadunund Date: Mon, 28 Oct 2024 17:39:54 +0100 Subject: [PATCH] Add nexus_workcell_editor (#36) * Add nexus_workcell_editor Signed-off-by: Yadunund * Fix demo workcell Signed-off-by: Luca Della Vedova * Test pin libm Signed-off-by: Luca Della Vedova * Revert "Test pin libm" This reverts commit 1055ce0290dae24fbfce4a0482e66e15a4fbec82. Signed-off-by: Luca Della Vedova --------- Signed-off-by: Yadunund Signed-off-by: Luca Della Vedova Co-authored-by: Luca Della Vedova --- .github/workflows/nexus_workcell_editor.yaml | 41 ++++ .github/workflows/style.yaml | 7 + README.md | 5 +- nexus_workcell_editor/.gitignore | 2 + nexus_workcell_editor/CHANGELOG.rst | 7 + nexus_workcell_editor/Cargo.toml | 21 ++ nexus_workcell_editor/README.md | 28 +++ nexus_workcell_editor/package.xml | 22 +++ nexus_workcell_editor/src/main.rs | 126 ++++++++++++ nexus_workcell_editor/src/main_menu.rs | 124 ++++++++++++ nexus_workcell_editor/src/ros_context.rs | 57 ++++++ .../src/workcell_calibration.rs | 183 ++++++++++++++++++ nexus_workcell_editor/test/test.workcell.json | 9 + 13 files changed, 630 insertions(+), 2 deletions(-) create mode 100644 .github/workflows/nexus_workcell_editor.yaml create mode 100644 nexus_workcell_editor/.gitignore create mode 100644 nexus_workcell_editor/CHANGELOG.rst create mode 100644 nexus_workcell_editor/Cargo.toml create mode 100644 nexus_workcell_editor/README.md create mode 100644 nexus_workcell_editor/package.xml create mode 100644 nexus_workcell_editor/src/main.rs create mode 100644 nexus_workcell_editor/src/main_menu.rs create mode 100644 nexus_workcell_editor/src/ros_context.rs create mode 100644 nexus_workcell_editor/src/workcell_calibration.rs create mode 100644 nexus_workcell_editor/test/test.workcell.json diff --git a/.github/workflows/nexus_workcell_editor.yaml b/.github/workflows/nexus_workcell_editor.yaml new file mode 100644 index 0000000..62e147d --- /dev/null +++ b/.github/workflows/nexus_workcell_editor.yaml @@ -0,0 +1,41 @@ +name: workcell_editor +on: + pull_request: + push: + branches: [ main ] +defaults: + run: + shell: bash +jobs: + test: + runs-on: ubuntu-latest + strategy: + matrix: + docker_image: ['ros:iron-ros-base'] + container: + image: ${{ matrix.docker_image }} + timeout-minutes: 30 + steps: + - name: Deps + run: | + apt update && apt install -y git curl libclang-dev libasound2-dev libudev-dev libgtk-3-dev python3-pip python3-vcstool + - name: Setup Rust + uses: dtolnay/rust-toolchain@1.75 + with: + components: clippy, rustfmt + - name: Install colcon cargo + run: | + cargo install --debug cargo-ament-build # --debug is faster to install + pip install git+https://github.com/colcon/colcon-cargo.git + pip install git+https://github.com/colcon/colcon-ros-cargo.git + - uses: actions/checkout@v2 + - name: vcs + run: | + git clone https://github.com/ros2-rust/ros2_rust.git -b 0.4.0 + vcs import . < ros2_rust/ros2_rust_iron.repos + - name: rosdep + run: | + rosdep update + rosdep install --from-paths . -yir + - name: build + run: /ros_entrypoint.sh colcon build --packages-up-to nexus_workcell_editor diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index fdeaabd..0654221 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -25,3 +25,10 @@ jobs: run: | sudo apt update && sudo apt install -y pycodestyle curl pycodestyle nexus_network_configuration/ + - name: Setup Rust + uses: dtolnay/rust-toolchain@1.75 + with: + components: clippy, rustfmt + - name: rustfmt + run: | + rustfmt --check --edition 2021 nexus_workcell_editor/src/main.rs diff --git a/README.md b/README.md index c9db9f4..84330b0 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # NEXUS -![](https://github.com/osrf/nexus/workflows/style/badge.svg) -![](https://github.com/osrf/nexus/workflows/integration_tests/badge.svg) +[![style](https://github.com/osrf/nexus/actions/workflows/style.yaml/badge.svg)](https://github.com/osrf/nexus/actions/workflows/style.yaml) +[![integration_tests](https://github.com/osrf/nexus/actions/workflows/nexus_integration_tests.yaml/badge.svg)](https://github.com/osrf/nexus/actions/workflows/nexus_integration_tests.yaml) +[![workcell_editor](https://github.com/osrf/nexus/actions/workflows/nexus_workcell_editor.yaml/badge.svg)](https://github.com/osrf/nexus/actions/workflows/nexus_workcell_editor.yaml) ![](./docs/media/nexus_architecture.png) diff --git a/nexus_workcell_editor/.gitignore b/nexus_workcell_editor/.gitignore new file mode 100644 index 0000000..2c96eb1 --- /dev/null +++ b/nexus_workcell_editor/.gitignore @@ -0,0 +1,2 @@ +target/ +Cargo.lock diff --git a/nexus_workcell_editor/CHANGELOG.rst b/nexus_workcell_editor/CHANGELOG.rst new file mode 100644 index 0000000..da94acc --- /dev/null +++ b/nexus_workcell_editor/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nexus_workcell_editor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +------------------ +* Provides the ``nexus_workcell_editor`` application to graphically design a workcell using individual components and calibrate the poses of these components. diff --git a/nexus_workcell_editor/Cargo.toml b/nexus_workcell_editor/Cargo.toml new file mode 100644 index 0000000..899d193 --- /dev/null +++ b/nexus_workcell_editor/Cargo.toml @@ -0,0 +1,21 @@ +[package] +name = "nexus_workcell_editor" +version = "0.0.1" +edition = "2021" + +[[bin]] +path = "src/main.rs" +name = "nexus_workcell_editor" + +[dependencies] +bevy = "0.12" +bevy_egui = "0.23" +# TODO(luca) Just use the version used by site editor once released +bevy_impulse = { git = "https://github.com/open-rmf/bevy_impulse", branch = "main" } +clap = { version = "4.0.10", features = ["color", "derive", "help", "usage", "suggestions"] } +crossbeam-channel = "0.5.8" +# TODO(luca) back to main when PR is merged +rmf_site_editor = { git = "https://github.com/open-rmf/rmf_site", rev = "f4bed77" } +rmf_site_format = { git = "https://github.com/open-rmf/rmf_site", rev = "f4bed77" } +rclrs = "0.4.0" +nexus_calibration_msgs = "*" diff --git a/nexus_workcell_editor/README.md b/nexus_workcell_editor/README.md new file mode 100644 index 0000000..51e7be6 --- /dev/null +++ b/nexus_workcell_editor/README.md @@ -0,0 +1,28 @@ +# nexus_workcell_editor + +A GUI for assembling workcells from components that is built off [rmf_site](https://github.com/open-rmf/rmf_site). + +## Setup + +Install rustup from the Rust website: https://www.rust-lang.org/tools/install + +``` +curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh +``` + +Follow instructions [here](https://github.com/ros2-rust/ros2_rust) to setup ros2_rust. + +## Build +``` +# source the ros distro and ros2_rust workspace. +cd ~/ws_nexus +colcon build +``` + +## Run +```bash +cd ~/ws_nexus +source ~/ws_nexus/install/setup.bash +ros2 run nexus_workcell_editor nexus_workcell_editor +``` + diff --git a/nexus_workcell_editor/package.xml b/nexus_workcell_editor/package.xml new file mode 100644 index 0000000..e71e60b --- /dev/null +++ b/nexus_workcell_editor/package.xml @@ -0,0 +1,22 @@ + + nexus_workcell_editor + 0.1.1 + A GUI to assemble workcells + Yadunund + Apache License 2.0 + + cargo + + gtk3 + libudev-dev + libasound2-dev + nexus_calibration_msgs + rclrs + + abb_irb910sc_support + nexus_assets + + + ament_cargo + + diff --git a/nexus_workcell_editor/src/main.rs b/nexus_workcell_editor/src/main.rs new file mode 100644 index 0000000..f9c1fb1 --- /dev/null +++ b/nexus_workcell_editor/src/main.rs @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2024 Johnson & Johnson + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use bevy::render::{ + render_resource::{AddressMode, SamplerDescriptor}, + settings::{WgpuFeatures, WgpuSettings}, + RenderPlugin, +}; +use bevy::{log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*}; +use bevy_egui::EguiPlugin; + +use clap::Parser; + +use librmf_site_editor::{ + aabb::AabbUpdatePlugin, animate::AnimationPlugin, asset_loaders::AssetLoadersPlugin, + interaction::InteractionPlugin, issue::IssuePlugin, keyboard::*, log::LogHistoryPlugin, + occupancy::OccupancyPlugin, site::SitePlugin, site_asset_io::SiteAssetIoPlugin, + view_menu::ViewMenuPlugin, widgets::*, wireframe::SiteWireframePlugin, + workcell::WorkcellEditorPlugin, workspace::*, AppState, CommandLineArgs, +}; + +pub mod main_menu; +use main_menu::{Autoload, MainMenuPlugin}; + +pub mod ros_context; +use ros_context::RosContextPlugin; + +pub mod workcell_calibration; +use workcell_calibration::WorkcellCalibrationPlugin; + +fn main() { + info!("Starting nexus_workcell_editor"); + + let mut app = App::new(); + + #[cfg(not(target_arch = "wasm32"))] + { + let command_line_args: Vec = std::env::args().collect(); + let command_line_args = CommandLineArgs::parse_from(command_line_args); + if let Some(path) = command_line_args.filename { + app.insert_resource(Autoload::file( + path.into(), + command_line_args.import.map(Into::into), + )); + } + } + + let mut plugins = DefaultPlugins.build(); + plugins = plugins.set(WindowPlugin { + primary_window: Some(Window { + title: "RMF Site Editor".to_owned(), + #[cfg(not(target_arch = "wasm32"))] + resolution: (1600., 900.).into(), + #[cfg(target_arch = "wasm32")] + canvas: Some(String::from("#rmf_site_editor_canvas")), + #[cfg(target_arch = "wasm32")] + fit_canvas_to_parent: true, + ..default() + }), + ..default() + }); + app.add_plugins(( + SiteAssetIoPlugin, + plugins + .disable::() + .set(ImagePlugin { + default_sampler: SamplerDescriptor { + address_mode_u: AddressMode::Repeat, + address_mode_v: AddressMode::Repeat, + address_mode_w: AddressMode::Repeat, + ..Default::default() + } + .into(), + }) + .set(RenderPlugin { + render_creation: WgpuSettings { + #[cfg(not(target_arch = "wasm32"))] + features: WgpuFeatures::POLYGON_MODE_LINE, + ..default() + } + .into(), + ..default() + }), + )); + + app.insert_resource(DirectionalLightShadowMap { size: 2048 }) + .add_state::() + .add_plugins(( + AssetLoadersPlugin, + LogHistoryPlugin, + AabbUpdatePlugin, + EguiPlugin, + KeyboardInputPlugin, + MainMenuPlugin, + WorkcellEditorPlugin, + SitePlugin, + InteractionPlugin::default(), + StandardUiPlugin::default(), + AnimationPlugin, + OccupancyPlugin, + WorkspacePlugin, + SiteWireframePlugin, + )) + .add_plugins(( + IssuePlugin, + ViewMenuPlugin, + RosContextPlugin, + WorkcellCalibrationPlugin, + bevy_impulse::ImpulsePlugin::default(), + )) + .run(); +} diff --git a/nexus_workcell_editor/src/main_menu.rs b/nexus_workcell_editor/src/main_menu.rs new file mode 100644 index 0000000..3d2ee45 --- /dev/null +++ b/nexus_workcell_editor/src/main_menu.rs @@ -0,0 +1,124 @@ +/* + * Copyright (C) 2024 Johnson & Johnson + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use bevy::{app::AppExit, prelude::*, tasks::Task, window::PrimaryWindow}; +use bevy_egui::{egui, EguiContexts}; +use librmf_site_editor::{ + workspace::{WorkspaceData, WorkspaceLoader}, + AppState, +}; +use rmf_site_format; +use std::path::PathBuf; + +#[derive(Resource)] +pub struct Autoload { + pub filename: Option, + pub import: Option, + pub importing: Option>>, +} + +impl Autoload { + pub fn file(filename: PathBuf, import: Option) -> Self { + Autoload { + filename: Some(filename), + import, + importing: None, + } + } +} + +pub fn demo_workcell() -> Vec { + return include_str!("../test/test.workcell.json") + .as_bytes() + .to_vec(); +} + +fn egui_ui( + mut egui_context: EguiContexts, + mut _exit: EventWriter, + mut workspace_loader: WorkspaceLoader, + mut _app_state: ResMut>, + autoload: Option>, + primary_windows: Query>, +) { + if let Some(mut autoload) = autoload { + #[cfg(not(target_arch = "wasm32"))] + { + if let Some(filename) = autoload.filename.clone() { + workspace_loader.load_from_path(filename); + } + autoload.filename = None; + } + return; + } + + let Some(ctx) = primary_windows + .get_single() + .ok() + .and_then(|w| egui_context.try_ctx_for_window_mut(w)) + else { + return; + }; + egui::Window::new("Welcome!") + .collapsible(false) + .resizable(false) + .title_bar(false) + .anchor(egui::Align2::CENTER_CENTER, egui::vec2(0., 0.)) + .show(ctx, |ui| { + ui.heading("Welcome to The NEXUS Workcell Editor!"); + ui.add_space(10.); + + ui.horizontal(|ui| { + if ui.button("New workcell").clicked() { + workspace_loader.load_from_data(WorkspaceData::Workcell( + rmf_site_format::Workcell::default() + .to_string() + .unwrap() + .into(), + )); + } + + if ui.button("Load workcell").clicked() { + workspace_loader.load_from_dialog(); + } + + if ui.button("Demo workcell").clicked() { + workspace_loader.load_from_data(WorkspaceData::Workcell(demo_workcell())); + } + }); + + #[cfg(not(target_arch = "wasm32"))] + { + ui.add_space(20.); + ui.horizontal(|ui| { + ui.with_layout(egui::Layout::right_to_left(egui::Align::Center), |ui| { + if ui.button("Exit").clicked() { + _exit.send(AppExit); + } + }); + }); + } + }); +} + +pub struct MainMenuPlugin; + +impl Plugin for MainMenuPlugin { + fn build(&self, app: &mut App) { + app.add_systems(Update, egui_ui.run_if(in_state(AppState::MainMenu))); + } +} diff --git a/nexus_workcell_editor/src/ros_context.rs b/nexus_workcell_editor/src/ros_context.rs new file mode 100644 index 0000000..2f26f09 --- /dev/null +++ b/nexus_workcell_editor/src/ros_context.rs @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2024 Johnson & Johnson + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use bevy::prelude::*; + +use std::sync::Arc; +use std::time::Duration; + +// The rclrs::context is a global variable and hence will be initialized as a +// bevy resource. +#[derive(Resource)] +pub struct RosContext { + context: rclrs::Context, + pub node: Arc, +} + +impl Default for RosContext { + fn default() -> Self { + let context = rclrs::Context::new(std::env::args()).unwrap_or_else(|err| { + panic!("Unable to initialize the ROS Context: {err}"); + }); + let node = rclrs::create_node(&context, "nexus_workcell_editor").unwrap_or_else(|err| { + panic!("Unable to initialize the ROS Node: {err}"); + }); + RosContext { context, node } + } +} + +fn spin_node(ros_context: Res) { + if ros_context.context.ok() { + let spin_node = Arc::clone(&ros_context.node); + let _ = rclrs::spin_once(spin_node, Some(Duration::ZERO)); + } +} + +pub struct RosContextPlugin; + +impl Plugin for RosContextPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .add_systems(Update, spin_node); + } +} diff --git a/nexus_workcell_editor/src/workcell_calibration.rs b/nexus_workcell_editor/src/workcell_calibration.rs new file mode 100644 index 0000000..2cd30df --- /dev/null +++ b/nexus_workcell_editor/src/workcell_calibration.rs @@ -0,0 +1,183 @@ +/* + * Copyright (C) 2024 Johnson & Johnson + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use bevy::prelude::*; +use bevy_egui::EguiContexts; + +use crossbeam_channel::{Receiver, Sender}; + +use rmf_site_format::{anchor::*, workcell::*}; + +use std::{collections::HashMap, sync::Arc}; + +use nexus_calibration_msgs::srv::CalibrateExtrinsics; +use nexus_calibration_msgs::srv::CalibrateExtrinsics_Request; +use nexus_calibration_msgs::srv::CalibrateExtrinsics_Response; + +use crate::ros_context::*; + +// This component will be attached to a workcell entity. +#[derive(Component)] +pub struct CalibrationClient { + pub client: Arc>, +} + +type TransformData = HashMap; +// Channels to send transform data from a system that makes the service reqeust +// to a system that will update anchor poses. +#[derive(Debug, Resource)] +pub struct CalibrationChannels { + pub sender: Sender, + pub receiver: Receiver, +} + +impl Default for CalibrationChannels { + fn default() -> Self { + let (sender, receiver) = crossbeam_channel::unbounded(); + Self { sender, receiver } + } +} + +// A system to insert the CalibrationClient component into a workcell entity. +pub fn add_calibration_client( + mut commands: Commands, + workcells: Query<(Entity, &NameOfWorkcell), Changed>, + ros_context: Res, +) { + for (e, component) in &workcells { + let base_service_name: &str = "/calibrate_extrinsics"; + let service_name = component.0.clone() + base_service_name; + let client = ros_context + .node + .create_client::(service_name.as_str()) + .unwrap_or_else(|err| { + panic!("Unable to create service client {err}"); + }); + // This will overwrite any previous value(s) of the same component type. + commands.entity(e).insert(CalibrationClient { client }); + } +} + +// A system to make the calibration service call and update anchors +fn handle_keyboard_input( + keyboard_input: Res>, + calib_channels: ResMut, + mut egui_context: EguiContexts, + workcells: Query<(Entity, &CalibrationClient, &NameOfWorkcell)>, +) { + let egui_context = egui_context.ctx_mut(); + let ui_has_focus = egui_context.wants_pointer_input() + || egui_context.wants_keyboard_input() + || egui_context.is_pointer_over_area(); + + if ui_has_focus { + return; + } + + if !keyboard_input.just_pressed(KeyCode::C) { + return; + } + info!("Calibrating workcells..."); + for (_e, calibration, workcell) in workcells.iter() { + // Make service call here and put frame names into a hash map. + // As per workcell coordinate frame conventions, the workcell + // datum link is named as _workcell_link. + let base_workcell_root_name: &str = "_workcell_link"; + let request = CalibrateExtrinsics_Request { + frame_id: workcell.0.clone() + base_workcell_root_name, + }; + dbg!(&request); + + let sender = calib_channels.sender.clone(); + calibration + .client + .async_send_request_with_callback( + request, + move |response: CalibrateExtrinsics_Response| { + if !response.success { + error!("Unsuccessful calibration results"); + return; + } + info!("Successfully retrieved calibration results!"); + let mut transforms = TransformData::new(); + for r in response.results { + transforms.insert( + r.child_frame_id, + Transform { + translation: Vec3::new( + r.transform.translation.x as f32, + r.transform.translation.y as f32, + r.transform.translation.z as f32, + ), + rotation: Quat::from_xyzw( + r.transform.rotation.x as f32, + r.transform.rotation.y as f32, + r.transform.rotation.z as f32, + r.transform.rotation.w as f32, + ), + scale: Vec3::ONE, + }, + ); + } + sender + .send(transforms) + .expect("Failed sending calibration transforms"); + }, + ) + .unwrap_or_else(|_err| { + panic!("Unable to get calibration results"); + }); + } +} + +fn update_anchor_poses( + calib_channels: ResMut, + mut anchors: Query<(&NameInWorkcell, &mut Anchor)>, +) { + if let Ok(result) = calib_channels.receiver.try_recv() { + for (name, mut anchor) in &mut anchors { + match result.get(&name.0) { + Some(t) => { + anchor.move_to(t); + info!( + "Moving anchor {} to [pos: {}, rot:{}]", + &name.0, &t.translation, &t.rotation + ); + } + None => { + warn!("[warn] No calibration data received for link {}", &name.0); + continue; + } + } + } + } +} + +pub struct WorkcellCalibrationPlugin; + +impl Plugin for WorkcellCalibrationPlugin { + fn build(&self, app: &mut App) { + app.init_resource::().add_systems( + Update, + ( + add_calibration_client, + handle_keyboard_input, + update_anchor_poses, + ), + ); + } +} diff --git a/nexus_workcell_editor/test/test.workcell.json b/nexus_workcell_editor/test/test.workcell.json new file mode 100644 index 0000000..e3981b4 --- /dev/null +++ b/nexus_workcell_editor/test/test.workcell.json @@ -0,0 +1,9 @@ +{ + "name": "workcell_1", + "id": 0, + "frames": {}, + "visuals": {}, + "collisions": {}, + "inertias": {}, + "joints": {} +}