diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index f9aadcd..620c45b 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.github/workflows/nexus_integration_tests.yaml @@ -11,7 +11,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - docker_image: ['ros:iron-ros-base'] + docker_image: ['ros:jazzy-ros-base'] container: image: ${{ matrix.docker_image }} timeout-minutes: 60 @@ -19,10 +19,6 @@ jobs: - name: Install deps for Rust run: | apt update && apt install -y git curl libclang-dev - - name: Setup Rust for nexus_zenoh_bridge - uses: dtolnay/rust-toolchain@stable - with: - components: clippy, rustfmt - uses: actions/checkout@v2 - uses: actions/cache@v3 with: @@ -40,7 +36,7 @@ jobs: rosdep update rosdep install --from-paths . -yir - name: build - run: /ros_entrypoint.sh colcon build --packages-up-to nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache + run: /ros_entrypoint.sh colcon build --packages-up-to nexus_calibration nexus_gazebo nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - name: Test - Unit Tests run: . ./install/setup.bash && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+ - name: Test - Integration Test diff --git a/.github/workflows/nexus_workcell_editor.yaml b/.github/workflows/nexus_workcell_editor.yaml new file mode 100644 index 0000000..73e5357 --- /dev/null +++ b/.github/workflows/nexus_workcell_editor.yaml @@ -0,0 +1,45 @@ +name: workcell_editor +on: + pull_request: + push: + branches: [ main ] +defaults: + run: + shell: bash +jobs: + test: + runs-on: ubuntu-latest + strategy: + matrix: + docker_image: ['ros:jazzy-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 colcon-cargo --break-system-packages + pip install colcon-ros-cargo --break-system-packages + - uses: actions/checkout@v2 + - name: vcs + # TODO(luca) Go back to cloning a tag when a new version is released with jazzy repos file + run: | + git clone https://github.com/ros2-rust/ros2_rust.git + cd ros2_rust + git checkout f45a66f47dc727e3ccb13037a6c57923af1446c7 + cd .. + vcs import . < ros2_rust/ros2_rust_jazzy.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..07b3bdb 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -11,17 +11,25 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - docker_image: ['ros:iron-ros-base'] + docker_image: ['ros:jazzy-ros-base'] container: image: ${{ matrix.docker_image }} steps: - name: checkout uses: actions/checkout@v2 - - name: uncrustify - run: | - sudo apt update && sudo apt install -y ros-iron-rmf-utils - /ros_entrypoint.sh ament_uncrustify -c /opt/ros/iron/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp + # TODO(luca) reintroduce after formatting + #- name: uncrustify + # run: | + # sudo apt update && sudo apt install -y ros-jazzy-rmf-utils + # /ros_entrypoint.sh ament_uncrustify -c /opt/ros/jazzy/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp - name: pycodestyle 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..e3b1f40 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) @@ -9,7 +10,7 @@ A ROS 2 framework which enables configuration and orchestration of process workf For details on architecture and concepts [see](./docs/concepts.md). ## Requirements -* [ROS 2 Iron](https://docs.ros.org/en/iron/Installation/Ubuntu-Install-Debians.html) on `Ubuntu 22.04` +* [ROS 2 Jazzy](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debians.html) on `Ubuntu 24.04` ## Setup @@ -33,12 +34,12 @@ cd ~/ws_nexus/src/ git clone git@github.com:osrf/nexus vcs import . < nexus/abb.repos cd ~/ws_nexus -rosdep install --from-paths src --ignore-src --rosdistro iron -y -r +rosdep install --from-paths src --ignore-src --rosdistro jazzy -y -r ``` ### Build the NEXUS workspace ```bash -source /opt/ros/iron/setup.bash +source /opt/ros/jazzy/setup.bash colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release ``` @@ -102,10 +103,10 @@ The linter of choice is `uncrustify` and the configuration used may be reference Instead of invoking `uncrustify` directly, use `ament_uncrustify` instead which is a wrapper around a specific version of `uncrustify`. You may locally run the linter as follows ```bash -sudo apt update && sudo apt install -y ros-iron-rmf-utils # This is a one-time step -source /opt/ros/iron/setup.bash +sudo apt update && sudo apt install -y ros-jazzy-rmf-utils # This is a one-time step +source /opt/ros/jazzy/setup.bash cd ~/ws_nexus/src/nexus -ament_uncrustify -c /opt/ros/iron/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp +ament_uncrustify -c /opt/ros/jazzy/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp ``` To automatically reformat the code, append `--reformat` to the `ament_uncrustify` line above. It is highly recommended to audit the changes by the linter before committing. diff --git a/nexus_calibration/CHANGELOG.rst b/nexus_calibration/CHANGELOG.rst new file mode 100644 index 0000000..fc75704 --- /dev/null +++ b/nexus_calibration/CHANGELOG.rst @@ -0,0 +1,10 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nexus_calibration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2023-11-22) +------------------ + +0.1.0 (2023-11-06) +------------------ +* Provides ``nexus_calibration_node`` which can be queried for poses of calibration links within a workcell. diff --git a/nexus_calibration/CMakeLists.txt b/nexus_calibration/CMakeLists.txt new file mode 100644 index 0000000..9391c66 --- /dev/null +++ b/nexus_calibration/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.8) +project(nexus_calibration) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +set(dep_pkgs + nexus_endpoints + rclcpp + rclcpp_components + rclcpp_lifecycle + tf2 + VRPN +) +foreach(pkg ${dep_pkgs}) + find_package(${pkg} REQUIRED) +endforeach() + +#=============================================================================== +add_library(nexus_calibration_component SHARED src/calibration_node.cpp) + +ament_target_dependencies(nexus_calibration_component ${dep_pkgs}) + +target_compile_features(nexus_calibration_component INTERFACE cxx_std_17) + +rclcpp_components_register_node(nexus_calibration_component + PLUGIN "nexus::CalibrationNode" + EXECUTABLE nexus_calibration_node + EXECUTOR SingleThreadedExecutor) + + +#=============================================================================== +if(BUILD_TESTING) + +endif() + +#=============================================================================== +install( + TARGETS + nexus_calibration_component + RUNTIME DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +ament_package() diff --git a/nexus_calibration/README.md b/nexus_calibration/README.md new file mode 100644 index 0000000..5c6b9b2 --- /dev/null +++ b/nexus_calibration/README.md @@ -0,0 +1,15 @@ +## nexus_calibration + +This package provides a ROS 2 Lifecyle node, `nexus_calibration_node`, that connects to a [VRPN](https://vrpn.github.io/) server and builds a cache of rigid body poses in a local `TF2` buffer. +These rigid bodies could represent calibration links (or reference links) on components within a workcell. +The poses of these links can then be queried via a ROS 2 service call over the `nexus::endpoints::ExtrinsicCalibrationService` endpoint. + +## Test +```bash +cd nexus_calibration/ +ros2 launch test/nexus_calibration.launch.py +`` +Then to retrieve poses of components wrt to the `robot_calibration_link`, ie a reference frame on the robot's `base_link`, +```bash +ros2 service call /workcell_1/calibrate_extrinsics nexus_calibration_msgs/src/CalibrateExtrinsics '{frame_id: robot_calibration_link}' +``` diff --git a/nexus_calibration/config/sample_config.yaml b/nexus_calibration/config/sample_config.yaml new file mode 100644 index 0000000..b5d3088 --- /dev/null +++ b/nexus_calibration/config/sample_config.yaml @@ -0,0 +1,7 @@ +nexus_calibration_node: + ros__parameters: + vrpn_server_address: "localhost:3883" # The ip_address:port of the vrpn server. + vrpn_frame_id: "world" # The calibrated reference frame wrt which the vrpn server is streaming data. + workcell_id: "workcell_1" # The name of the workcell. This is used as a prefix for any endpoints. + tracker_names: ["workcell_calibration_link", "robot_calibration_link"] # The names of the relevant rigid bodies trackers. + update_rate: 2.0 # The rate in hz at which the update loop should run. diff --git a/nexus_calibration/package.xml b/nexus_calibration/package.xml new file mode 100644 index 0000000..8926f9d --- /dev/null +++ b/nexus_calibration/package.xml @@ -0,0 +1,24 @@ + + + + nexus_calibration + 0.1.1 + A package with ROS 2 nodes for calibration workcell components + Yadunund + Apache License 2.0 + + ament_cmake + + nexus_endpoints + rclcpp + rclcpp_components + rclcpp_lifecycle + tf2 + vrpn + + nexus_gazebo + + + ament_cmake + + diff --git a/nexus_calibration/src/calibration_node.cpp b/nexus_calibration/src/calibration_node.cpp new file mode 100644 index 0000000..ca1f8c4 --- /dev/null +++ b/nexus_calibration/src/calibration_node.cpp @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2023 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. + * + */ + +#include "calibration_node.hpp" + +namespace nexus { + +//============================================================================== +struct CalibrationNode::Tracker +{ + std::string name; + std::string vrpn_frame_id; + std::unique_ptr vrpn_tracker; + std::shared_ptr buffer; + + Tracker( + const std::string& name_, + const std::string& vrpn_frame_id_, + std::shared_ptr connection, + std::shared_ptr buffer_) + : name(name_), + vrpn_frame_id(vrpn_frame_id_), + buffer(buffer_) + { + vrpn_tracker = + std::make_unique(name.c_str(), connection.get()); + vrpn_tracker->register_change_handler(this, &Tracker::handle_pose); + } + + // This callback will write pose information as a static transform into + // the buffer. + static void VRPN_CALLBACK handle_pose( + void* userData, + const vrpn_TRACKERCB tracker_pose) + { + Tracker* tracker = static_cast(userData); + + TransformStamped transform; + transform.header.frame_id = tracker->vrpn_frame_id; + transform.header.stamp.sec = tracker_pose.msg_time.tv_sec; + transform.header.stamp.nanosec = tracker_pose.msg_time.tv_usec * 1000; + transform.child_frame_id = tracker->name; + auto& translation = transform.transform.translation; + translation.x = tracker_pose.pos[0]; + translation.y = tracker_pose.pos[1]; + translation.z = tracker_pose.pos[2]; + auto& quat = transform.transform.rotation; + quat.x = tracker_pose.quat[0]; + quat.y = tracker_pose.quat[1]; + quat.z = tracker_pose.quat[2]; + quat.w = tracker_pose.quat[3]; + + tracker->buffer->setTransform( + std::move(transform), + tracker->name, + true // static + ); + } + + ~Tracker() + { + vrpn_tracker->unregister_change_handler(this, &Tracker::handle_pose); + } +}; + +//============================================================================== +CalibrationNode::CalibrationNode(const rclcpp::NodeOptions& options) +: LifecycleNode("nexus_calibration_node", options), + _vrpn_connection(nullptr) +{ + RCLCPP_INFO( + this->get_logger(), + "Running nexus_calibration_node..." + ); + + _vrpn_frame_id = this->declare_parameter("vrpn_frame_id", "world"); + _tracker_names = this->declare_parameter("tracker_names", _tracker_names); + { + ParameterDescriptor desc; + desc.description = "The vrpn server address as ip_address:port."; + _server_address = + this->declare_parameter("vrpn_server_address", desc); + } + _update_rate = this->declare_parameter("update_rate", 2.0); + _workcell_id = this->declare_parameter("workcell_id"); +} + +//============================================================================== +auto CalibrationNode::on_configure(const State& /*previous_state*/) +-> CallbackReturn +{ + RCLCPP_INFO(this->get_logger(), "Configuring..."); + + try + { + // The vrpn connection will attempt to connect when its mainloop is run. + _vrpn_connection = std::shared_ptr( + vrpn_get_connection_by_name(_server_address.c_str())); + + _tf_buffer = std::make_shared(); + + // Create trackers + for (const auto& t : _tracker_names) + { + auto it = _trackers.insert({t, nullptr}); + if (!it.second) + { + // Skip duplicates. + RCLCPP_WARN( + this->get_logger(), + "Duplicate tracker [%s] provided. Ignoring...", + t.c_str() + ); + continue; + } + it.first->second = std::make_shared( + t, + this->_vrpn_frame_id, + this->_vrpn_connection, + this->_tf_buffer + ); + } + } + catch (const std::exception& e) + { + RCLCPP_ERROR( + this->get_logger(), + "Error while setting up vrpn connection: %s", + e.what() + ); + return CallbackReturn::FAILURE; + } + + if (_workcell_id.empty()) + { + RCLCPP_ERROR( + this->get_logger(), + "Parameter workcell_id cannot be empty." + ); + return CallbackReturn::FAILURE; + } + + if (_vrpn_frame_id.empty()) + { + RCLCPP_ERROR( + this->get_logger(), + "Parameter vrpn_frame_id cannot be empty." + ); + return CallbackReturn::FAILURE; + } + + _extrinsic_service = this->create_service( + ExtrinsicCalibrationService::service_name(_workcell_id), + [this]( + ExtrinsicCalibrationServiceType::Request::ConstSharedPtr req, + ExtrinsicCalibrationServiceType::Response::SharedPtr res) + { + res->success = false; + if (this->get_current_state().label() != "active") + { + RCLCPP_ERROR( + this->get_logger(), + "ExtrinsicCalibrationService called before node activation!" + ); + return; + } + try + { + if (req->frame_id.empty()) + { + RCLCPP_ERROR( + this->get_logger(), + "frame_id cannot be empty in ExtrinsicCalibration request." + ); + return; + } + for (const auto& [tracker_name, _] : _trackers) + { + res->results.emplace_back(_tf_buffer->lookupTransform( + req->frame_id, + tracker_name, + tf2::TimePointZero + )); + } + } + catch (const std::exception& e) + { + RCLCPP_ERROR( + this->get_logger(), + "Error when looking up transform. Details: %s", + e.what() + ); + return; + } + + res->success = true; + } + ); + + // Start update timer. + const auto timer_period = + std::chrono::duration_cast( + std::chrono::duration>(1.0 / _update_rate)); + this->_timer = this->create_wall_timer( + timer_period, + [this]() + { + if (this->get_current_state().label() != "active") + { + return; + } + + this->_vrpn_connection->mainloop(); + RCLCPP_DEBUG( + this->get_logger(), + "Updated connection mainLoop." + ); + if (!this->_vrpn_connection->doing_okay()) + { + RCLCPP_WARN( + this->get_logger(), + "VRPN connection to server %s is not doing okay...", + _server_address.c_str() + ); + return; + } + for (const auto& [t, tracker] : _trackers) + { + tracker->vrpn_tracker->mainloop(); + RCLCPP_DEBUG( + this->get_logger(), + "Updated mainloop for tracker %s.", + t.c_str() + ); + } + }); + + RCLCPP_INFO(this->get_logger(), "Successfully configured."); + return CallbackReturn::SUCCESS; +} + +//============================================================================== +auto CalibrationNode::on_cleanup(const State& /*previous_state*/) +-> CallbackReturn +{ + RCLCPP_INFO(this->get_logger(), "Cleaning up..."); + _trackers.clear(); + _tf_buffer.reset(); + _vrpn_connection.reset(); + _timer.reset(); + _extrinsic_service.reset(); + RCLCPP_INFO(this->get_logger(), "Successfully cleaned up."); + return CallbackReturn::SUCCESS; +} + +//============================================================================== +auto CalibrationNode::on_shutdown(const State& /*previous_state*/) +-> CallbackReturn +{ + RCLCPP_INFO(this->get_logger(), "Shutting down..."); + return CallbackReturn::SUCCESS; +} + +//============================================================================== +auto CalibrationNode::on_activate(const State& /*previous_state*/) +-> CallbackReturn +{ + RCLCPP_INFO(this->get_logger(), "Activating..."); + RCLCPP_INFO(this->get_logger(), "Successfully activated."); + return CallbackReturn::SUCCESS; +} + +//============================================================================== +auto CalibrationNode::on_deactivate(const State& /*previous_state*/) +-> CallbackReturn +{ + RCLCPP_INFO(this->get_logger(), "Deactivating..."); + RCLCPP_INFO(this->get_logger(), "Successfully deactivated."); + return CallbackReturn::SUCCESS; +} + +//============================================================================== +auto CalibrationNode::on_error(const State& /*previous_state*/) +-> CallbackReturn +{ + RCLCPP_ERROR(this->get_logger(), "Error!"); + return CallbackReturn::SUCCESS; +} + +//============================================================================== +CalibrationNode::~CalibrationNode() +{ + // Delete all references within Tracker. + _trackers.clear(); +} + +} // namespace nexus + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(nexus::CalibrationNode) diff --git a/nexus_calibration/src/calibration_node.hpp b/nexus_calibration/src/calibration_node.hpp new file mode 100644 index 0000000..75d4a9b --- /dev/null +++ b/nexus_calibration/src/calibration_node.hpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2023 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. + * + */ + +#ifndef SRC__CALIBRATION_NODE_HPP +#define SRC__CALIBRATION_NODE_HPP + +#include + +#include + +#include +#include + +#include + +// VRPN headers +#include +#include + +// ============================================================================= +namespace nexus { + +class CalibrationNode : public rclcpp_lifecycle::LifecycleNode +{ + +public: + + using CallbackReturn = rclcpp_lifecycle::LifecycleNode::CallbackReturn; + using ExtrinsicCalibrationService = endpoints::ExtrinsicCalibrationService; + using ExtrinsicCalibrationServiceType = + ExtrinsicCalibrationService::ServiceType; + using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor; + using State = rclcpp_lifecycle::State; + using TransformStamped = geometry_msgs::msg::TransformStamped; + + /// @brief Constructor + /// @param options NodeOptions + CalibrationNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + /// rclcpp_lifecycle::LifecycleNode functions to override + CallbackReturn on_configure(const State& previous_state) override; + CallbackReturn on_cleanup(const State& previous_state) override; + CallbackReturn on_shutdown(const State& previous_state) override; + CallbackReturn on_activate(const State& previous_state) override; + CallbackReturn on_deactivate(const State& previous_state) override; + CallbackReturn on_error(const State& previous_state) override; + + ~CalibrationNode(); + +private: + + // Forward declare. + struct Tracker; + using TrackerPtr = std::shared_ptr; + + // Map tracker name to its Tracker. + std::unordered_map _trackers; + + // All trackers will dump their frames into this buffer. + std::shared_ptr _tf_buffer; + + // VRPN connection + std::shared_ptr _vrpn_connection; + + // The frame_id in which tracking data is being streamed. Usually world. + std::string _vrpn_frame_id = "world"; + + // The name of the workcell which is being calibrated. + std::string _workcell_id; + + // Destination of server in ip:port format. + std::string _server_address; + + // Tracker names + std::vector _tracker_names; + + // Timer to run the vrvpn API's update functions. + // TODO(YV): Consider disabling this timer once good quality updates + // are received to minimize CPU load. + rclcpp::TimerBase::SharedPtr _timer; + + // Rate for timer. + double _update_rate; + + // Service server + rclcpp::Service::SharedPtr + _extrinsic_service; + +}; + +} // namespace nexus + + +#endif // SRC__CALIBRATION_NODE_HPP diff --git a/nexus_calibration/test/nexus_calibration.launch.py b/nexus_calibration/test/nexus_calibration.launch.py new file mode 100644 index 0000000..a2eb601 --- /dev/null +++ b/nexus_calibration/test/nexus_calibration.launch.py @@ -0,0 +1,134 @@ +# Copyright (C) 2023 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. + +import os +import pathlib +from ament_index_python.packages import get_package_share_directory + +import launch_ros +from launch_ros.actions import LifecycleNode +from launch_ros.event_handlers import OnStateTransition +import launch +from launch.actions import ( + GroupAction, + OpaqueFunction, + ExecuteProcess, +) +import lifecycle_msgs + +def activate_node(target_node: LifecycleNode, depend_node: LifecycleNode = None): + + configure_event = None + + if depend_node is not None: + configure_event = launch.actions.RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=depend_node, + goal_state="active", + entities=[ + launch.actions.EmitEvent( + event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action( + target_node + ), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + ], + ) + ) + else: + # Make the talker node take the 'configure' transition. + configure_event = launch.actions.EmitEvent( + event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action(target_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + + inactive_state_handler = launch.actions.RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=target_node, + goal_state="inactive", + entities=[ + launch.actions.LogInfo( + msg=f"node {target_node.node_executable} reached the 'inactive' state." + ), + launch.actions.EmitEvent( + event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action( + target_node + ), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ) + ), + ], + ) + ) + + active_state_handler = launch.actions.RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=target_node, + goal_state="active", + entities=[ + launch.actions.LogInfo( + msg=f"node {target_node.node_executable} reached the 'active' state" + ), + ], + ) + ) + + return GroupAction( + [ + configure_event, + inactive_state_handler, + active_state_handler, + ] + ) + +def launch_setup(context, *args, **kwargs): + curdir = pathlib.Path(__file__).parent.resolve() + config_path = os.path.join(curdir, "test_config.yaml") + world_path = os.path.join(curdir, "test_world.world") + + simulation = ExecuteProcess( + # -v verbose + # -s server only + # -r run on start + name="Gazebo", + cmd=['ign gazebo -v 4 -r ', world_path], + output='screen', + shell=True, + ) + + calibration_node = LifecycleNode( + namespace="", + package="nexus_calibration", + executable="nexus_calibration_node", + name="nexus_calibration_node", + parameters=[config_path] + ) + + return [ + simulation, + calibration_node, + activate_node(calibration_node) + ] + +def generate_launch_description(): + return launch.LaunchDescription( + [ + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/nexus_calibration/test/test_config.yaml b/nexus_calibration/test/test_config.yaml new file mode 100644 index 0000000..5b93712 --- /dev/null +++ b/nexus_calibration/test/test_config.yaml @@ -0,0 +1,7 @@ +nexus_calibration_node: + ros__parameters: + vrpn_server_address: "localhost:3883" # The ip_address:port of the vrpn server. + vrpn_frame_id: "workcell_1_workcell_link" # The calibrated reference frame wrt which the vrpn server is streaming data. + workcell_id: "workcell_1" # The name of the workcell. This is used as a prefix for any endpoints. + tracker_names: ["table_calibration_link", "robot_calibration_link", "dispenser_calibration_link", "pallet_calibration_link"] # The names of the relevant rigid bodies trackers. + update_rate: 2.0 # The rate in hz at which the update loop should run. diff --git a/nexus_calibration/test/test_world.world b/nexus_calibration/test/test_world.world new file mode 100644 index 0000000..b0812a6 --- /dev/null +++ b/nexus_calibration/test/test_world.world @@ -0,0 +1,55 @@ + + + + + 0 0 0 0 0 0 + true + + + 3883 + 50.0 + true + + + + + 0 0 0 0 0 1.5707 + true + + + 3884 + 50.0 + true + + + + + + model://motion_capture_rigid_body + table_calibration_link + 0.5 0.3 0 0 0 0 + true + + + + model://motion_capture_rigid_body + robot_calibration_link + 0.1 -0.1 0.1 0 0 0 + true + + + + model://motion_capture_rigid_body + dispenser_calibration_link + 0.38 -0.4 0.02 0 0 0 + true + + + + model://motion_capture_rigid_body + pallet_calibration_link + 0.25 0.25 0.1 0 0 0 + true + + + diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt index 45e9e29..c9e590e 100644 --- a/nexus_capabilities/CMakeLists.txt +++ b/nexus_capabilities/CMakeLists.txt @@ -41,12 +41,13 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE CPP - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) endif() include(GNUInstallDirs) diff --git a/nexus_common/CMakeLists.txt b/nexus_common/CMakeLists.txt index 9fe26e3..3701b90 100644 --- a/nexus_common/CMakeLists.txt +++ b/nexus_common/CMakeLists.txt @@ -88,12 +88,13 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE CPP - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) # Adds a ament catch2 test with some common libraries. # Usage: nexus_add_test( ...) diff --git a/nexus_common/include/nexus_common/sync_service_client.hpp b/nexus_common/include/nexus_common/sync_service_client.hpp index 452b460..29dec20 100644 --- a/nexus_common/include/nexus_common/sync_service_client.hpp +++ b/nexus_common/include/nexus_common/sync_service_client.hpp @@ -36,7 +36,7 @@ public: template _cb_group(node->create_callback_group(rclcpp::CallbackGroupType:: MutuallyExclusive, false)), _client(node->template create_client( - service_name, rmw_qos_profile_services_default, this->_cb_group)) + service_name, rclcpp::ServicesQoS(), this->_cb_group)) { this->_executor.add_callback_group(this->_cb_group, node->get_node_base_interface()); diff --git a/nexus_common/package.xml b/nexus_common/package.xml index 16ede43..ce72257 100644 --- a/nexus_common/package.xml +++ b/nexus_common/package.xml @@ -19,10 +19,10 @@ yaml-cpp nexus_cmake + rmf_utils ament_cmake - rmf_utils ament_cmake_catch2 ament_cmake_uncrustify example_interfaces diff --git a/nexus_common/src/action_client_bt_node_test.cpp b/nexus_common/src/action_client_bt_node_test.cpp index 8d6e715..d5ad96a 100644 --- a/nexus_common/src/action_client_bt_node_test.cpp +++ b/nexus_common/src/action_client_bt_node_test.cpp @@ -16,7 +16,7 @@ */ #define CATCH_CONFIG_MAIN -#include +#include #include "nexus_common/action_client_bt_node.hpp" #include "nexus_common_test/test_utils.hpp" diff --git a/nexus_common/src/batch_service_call_test.cpp b/nexus_common/src/batch_service_call_test.cpp index 34ae1e3..a13176f 100644 --- a/nexus_common/src/batch_service_call_test.cpp +++ b/nexus_common/src/batch_service_call_test.cpp @@ -15,7 +15,7 @@ * */ -#include +#include #include "batch_service_call.hpp" #include "nexus_common_test/test_utils.hpp" diff --git a/nexus_common/src/logging_test.cpp b/nexus_common/src/logging_test.cpp index 57c1262..19b8bd1 100644 --- a/nexus_common/src/logging_test.cpp +++ b/nexus_common/src/logging_test.cpp @@ -16,7 +16,7 @@ */ #define CATCH_CONFIG_MAIN -#include +#include #include "logging.hpp" #include "test_utils.hpp" diff --git a/nexus_common/src/main_test.cpp b/nexus_common/src/main_test.cpp index 8d926bd..d97f1ce 100644 --- a/nexus_common/src/main_test.cpp +++ b/nexus_common/src/main_test.cpp @@ -16,4 +16,4 @@ */ #define CATCH_CONFIG_MAIN -#include +#include diff --git a/nexus_common/src/models/work_order_test.cpp b/nexus_common/src/models/work_order_test.cpp index e68839a..aa3d2be 100644 --- a/nexus_common/src/models/work_order_test.cpp +++ b/nexus_common/src/models/work_order_test.cpp @@ -15,7 +15,7 @@ * */ -#include +#include #include "models/work_order.hpp" diff --git a/nexus_common/src/pausable_sequence_test.cpp b/nexus_common/src/pausable_sequence_test.cpp index e9ca64f..ba72834 100644 --- a/nexus_common/src/pausable_sequence_test.cpp +++ b/nexus_common/src/pausable_sequence_test.cpp @@ -16,7 +16,7 @@ */ #define CATCH_CONFIG_MAIN -#include +#include #include "pausable_sequence.hpp" diff --git a/nexus_common/src/service_client_bt_node_test.cpp b/nexus_common/src/service_client_bt_node_test.cpp index 6abfba6..60af26b 100644 --- a/nexus_common/src/service_client_bt_node_test.cpp +++ b/nexus_common/src/service_client_bt_node_test.cpp @@ -16,7 +16,7 @@ */ #define CATCH_CONFIG_MAIN -#include +#include #include "nexus_common/service_client_bt_node.hpp" #include "nexus_common_test/test_utils.hpp" diff --git a/nexus_common/src/sync_ros_client_test.cpp b/nexus_common/src/sync_ros_client_test.cpp index 219ea3a..ece4ec8 100644 --- a/nexus_common/src/sync_ros_client_test.cpp +++ b/nexus_common/src/sync_ros_client_test.cpp @@ -16,7 +16,7 @@ */ #define CATCH_CONFIG_MAIN -#include +#include #include "sync_service_client.hpp" #include "test_utils.hpp" diff --git a/nexus_gazebo/CHANGELOG.rst b/nexus_gazebo/CHANGELOG.rst new file mode 100644 index 0000000..72e003f --- /dev/null +++ b/nexus_gazebo/CHANGELOG.rst @@ -0,0 +1,10 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nexus_gazebo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2023-11-22) +------------------ + +0.1.0 (2023-11-06) +------------------ +* Provides a motion capture plugin for Gazebo to track rigid bodies and broadcast their poses over VRPN. diff --git a/nexus_gazebo/CMakeLists.txt b/nexus_gazebo/CMakeLists.txt new file mode 100644 index 0000000..3d32ad1 --- /dev/null +++ b/nexus_gazebo/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) +project(nexus_gazebo) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(gz_sim_vendor REQUIRED) +find_package(gz-sim REQUIRED) +find_package(VRPN REQUIRED) + +#=============================================================================== +add_library(${PROJECT_NAME} SHARED + src/MotionCaptureSystem.cc + src/MotionCaptureRigidBody.cc +) + +target_link_libraries(${PROJECT_NAME} + gz-sim::core + ${VRPN_LIBRARIES} +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${VRPN_INCLUDE_DIRS} +) + +ament_target_dependencies(${PROJECT_NAME} + gz-sim + VRPN) + + +#=============================================================================== +if(BUILD_TESTING) + +endif() + +#=============================================================================== +install(DIRECTORY models/motion_capture_system DESTINATION share/${PROJECT_NAME}/models) +install(DIRECTORY models/motion_capture_rigid_body DESTINATION share/${PROJECT_NAME}/models) +install(DIRECTORY include/nexus_gazebo DESTINATION include/${PROJECT_NAME}) +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} + INCLUDES DESTINATION include/${PROJECT_NAME} +) + +ament_environment_hooks("nexus_gazebo_gazebo_paths.dsv.in") + +ament_package() diff --git a/nexus_gazebo/include/nexus_gazebo/Components.hh b/nexus_gazebo/include/nexus_gazebo/Components.hh new file mode 100644 index 0000000..f26aa29 --- /dev/null +++ b/nexus_gazebo/include/nexus_gazebo/Components.hh @@ -0,0 +1,36 @@ +// Copyright 2023 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. + +#ifndef NEXUS_GAZEBO__COMPONENTS_HH_ +#define NEXUS_GAZEBO__COMPONENTS_HH_ + +#include +#include +#include +#include +#include + +namespace nexus_gazebo::components { +using MotionCaptureRigidBody = gz::sim::components::Component< + std::string, + class MotionCaptureRigidBodyTag, + gz::sim::serializers::StringSerializer>; + +GZ_SIM_REGISTER_COMPONENT( + "nexus_gazebo.components.MotionCaptureRigidBody", + MotionCaptureRigidBody) + +} // namespace nexus_gazebo::components + +#endif // NEXUS_GAZEBO__COMPONENTS_HH_ diff --git a/nexus_gazebo/include/nexus_gazebo/MotionCaptureRigidBody.hh b/nexus_gazebo/include/nexus_gazebo/MotionCaptureRigidBody.hh new file mode 100644 index 0000000..264dda3 --- /dev/null +++ b/nexus_gazebo/include/nexus_gazebo/MotionCaptureRigidBody.hh @@ -0,0 +1,51 @@ +// Copyright 2023 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. + +#ifndef NEXUS_GAZEBO__MOTIONCAPTURERIGIDBODY_HH_ +#define NEXUS_GAZEBO__MOTIONCAPTURERIGIDBODY_HH_ + +#include +#include + +#include +#include + +namespace nexus_gazebo { + +using Entity = gz::sim::Entity; +using EntityComponentManager = gz::sim::EntityComponentManager; +using EventManager = gz::sim::EventManager; +using ISystemConfigure = gz::sim::ISystemConfigure; +using System = gz::sim::System; + +/// \class MotionCaptureRigidBody +class MotionCaptureRigidBody : + public System, + public ISystemConfigure +{ +public: + MotionCaptureRigidBody(); + + void Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& _eventMgr) override; + +private: + std::string rigid_body_label; +}; +} // namespace nexus_gazebo + +#endif // NEXUS_GAZEBO__MOTIONCAPTURERIGIDBODY_HH_ diff --git a/nexus_gazebo/include/nexus_gazebo/MotionCaptureSystem.hh b/nexus_gazebo/include/nexus_gazebo/MotionCaptureSystem.hh new file mode 100644 index 0000000..c9367b7 --- /dev/null +++ b/nexus_gazebo/include/nexus_gazebo/MotionCaptureSystem.hh @@ -0,0 +1,106 @@ +// Copyright 2023 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. + +#ifndef NEXUS_GAZEBO__MOTIONCAPTURESYSTEM_HH_ +#define NEXUS_GAZEBO__MOTIONCAPTURESYSTEM_HH_ + +#include +#include +#include +#include + +#include +#include + +#include "vrpn_Connection.h" +#include "vrpn_ConnectionPtr.h" +#include "vrpn_Tracker.h" + +namespace nexus_gazebo { + +using Entity = gz::sim::Entity; +using EntityComponentManager = gz::sim::EntityComponentManager; +using EventManager = gz::sim::EventManager; +using ISystemConfigure = gz::sim::ISystemConfigure; +using ISystemPostUpdate = gz::sim::ISystemPostUpdate; +using Pose3d = gz::math::Pose3d; +using System = gz::sim::System; +using UpdateInfo = gz::sim::UpdateInfo; + +class RigidBodyTracker : public vrpn_Tracker +{ +public: + explicit RigidBodyTracker( + const std::string& _tracker_name, + vrpn_Connection* _c = nullptr); + + ~RigidBodyTracker() override = default; + + void mainloop() override; + + void updatePose( + const std::chrono::steady_clock::duration& _sim_time, + const Pose3d& pose); + +private: + struct timeval timestamp; + Pose3d pose; +}; + +/// \class MotionCaptureSystem +class MotionCaptureSystem : + public System, + public ISystemConfigure, + public ISystemPostUpdate +{ +public: + /// \brief Constructor + MotionCaptureSystem(); + + /// \brief Destructor + ~MotionCaptureSystem() override; + + /// Configure the system + void Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& _eventMgr) override; + + /// Post-update callback + void PostUpdate( + const UpdateInfo& _info, + const EntityComponentManager& _ecm) override; + +private: + int port {3883}; + bool stream_rigid_bodies {true}; + bool stream_labeled_markers {true}; + bool stream_unlabeled_markers {true}; + double update_frequency {50.0}; + + /// Pointer to the VRPN connection + vrpn_ConnectionPtr connection {nullptr}; + + Entity entity {0}; + + std::chrono::steady_clock::duration last_pose_pub_time {0}; + + /// Collection of VRPN trackers + std::unordered_map> trackers; +}; + +} // namespace nexus_gazebo +#endif // NEXUS_GAZEBO__MOTIONCAPTURESYSTEM_HH_ diff --git a/nexus_gazebo/models/motion_capture_rigid_body/model.config b/nexus_gazebo/models/motion_capture_rigid_body/model.config new file mode 100644 index 0000000..88dd2f6 --- /dev/null +++ b/nexus_gazebo/models/motion_capture_rigid_body/model.config @@ -0,0 +1,15 @@ + + + motion_capture_rigid_body + 1.0 + motion_capture_rigid_body.sdf + + + Michael Carroll + michael@openrobotics.org + + + + A group of motion capture markers tracked as a rigid body + + diff --git a/nexus_gazebo/models/motion_capture_rigid_body/motion_capture_rigid_body.sdf b/nexus_gazebo/models/motion_capture_rigid_body/motion_capture_rigid_body.sdf new file mode 100644 index 0000000..61bd5b0 --- /dev/null +++ b/nexus_gazebo/models/motion_capture_rigid_body/motion_capture_rigid_body.sdf @@ -0,0 +1,26 @@ + + + + + + + + 0.0032 + + + + 0.9 0.9 0.9 1.0 + + + + + + + 0.0032 + + + + + + + diff --git a/nexus_gazebo/models/motion_capture_system/model.config b/nexus_gazebo/models/motion_capture_system/model.config new file mode 100644 index 0000000..8029750 --- /dev/null +++ b/nexus_gazebo/models/motion_capture_system/model.config @@ -0,0 +1,15 @@ + + + motion_capture_system + 1.0 + motion_capture_system.sdf + + + Michael Carroll + michael@openrobotics.org + + + + A motion capture system that publishes the poses of markers and rigid bodies over VRPN. + + diff --git a/nexus_gazebo/models/motion_capture_system/motion_capture_system.sdf b/nexus_gazebo/models/motion_capture_system/motion_capture_system.sdf new file mode 100644 index 0000000..34486e8 --- /dev/null +++ b/nexus_gazebo/models/motion_capture_system/motion_capture_system.sdf @@ -0,0 +1,13 @@ + + + true + + + 3883 + 50.0 + true + true + true + + + diff --git a/nexus_gazebo/nexus_gazebo_gazebo_paths.dsv.in b/nexus_gazebo/nexus_gazebo_gazebo_paths.dsv.in new file mode 100644 index 0000000..3b934f0 --- /dev/null +++ b/nexus_gazebo/nexus_gazebo_gazebo_paths.dsv.in @@ -0,0 +1,4 @@ +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;@CMAKE_INSTALL_PREFIX@/lib +prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models +prepend-non-duplicate;IGN_GAZEBO_SYSTEM_PLUGIN_PATH;@CMAKE_INSTALL_PREFIX@/lib diff --git a/nexus_gazebo/package.xml b/nexus_gazebo/package.xml new file mode 100644 index 0000000..7f4a705 --- /dev/null +++ b/nexus_gazebo/package.xml @@ -0,0 +1,25 @@ + + + + nexus_gazebo + 0.1.1 + Nexus gazebo simulation assets + + Michael Carroll + + Apache License 2.0 + + Michael Carroll + + ament_cmake + + rclcpp + vrpn + + gz_sim_vendor + gz_math_vendor + + + ament_cmake + + diff --git a/nexus_gazebo/src/MotionCaptureRigidBody.cc b/nexus_gazebo/src/MotionCaptureRigidBody.cc new file mode 100644 index 0000000..585f1fb --- /dev/null +++ b/nexus_gazebo/src/MotionCaptureRigidBody.cc @@ -0,0 +1,55 @@ +// Copyright 2023 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. + +#include "nexus_gazebo/MotionCaptureRigidBody.hh" +#include "nexus_gazebo/Components.hh" + +#include +#include +#include + +constexpr const char* kRigidBodyLabel = "rigid_body_label"; + +namespace nexus_gazebo { + +////////////////////////////////////////////////// +MotionCaptureRigidBody::MotionCaptureRigidBody() = default; + +void MotionCaptureRigidBody::Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) +{ + // Read SDF Configuration + this->rigid_body_label = _sdf->Get( + kRigidBodyLabel, this->rigid_body_label).first; + + if (this->rigid_body_label.empty()) + { + // In the case that the user hasn't overridden the label, then use + // the name of the entity we are attached to. + this->rigid_body_label = + _ecm.Component(_entity)->Data(); + } + + _ecm.CreateComponent(_entity, + components::MotionCaptureRigidBody(this->rigid_body_label)); +} +} // namespace nexus_gazebo + +GZ_ADD_PLUGIN( + nexus_gazebo::MotionCaptureRigidBody, + nexus_gazebo::System, + nexus_gazebo::MotionCaptureRigidBody::ISystemConfigure); diff --git a/nexus_gazebo/src/MotionCaptureSystem.cc b/nexus_gazebo/src/MotionCaptureSystem.cc new file mode 100644 index 0000000..1b2ee8a --- /dev/null +++ b/nexus_gazebo/src/MotionCaptureSystem.cc @@ -0,0 +1,171 @@ +// Copyright 2023 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. + +#include "nexus_gazebo/MotionCaptureSystem.hh" +#include "nexus_gazebo/Components.hh" + +#include +#include +#include + +constexpr const char* kPort = "port"; +constexpr const char* kSdfStreamRigid = "stream_rigid_bodies"; +constexpr const char* kSdfStreamLabeled = "stream_labeled_markers"; +constexpr const char* kSdfStreamUnlabeled = "stream_unlabeled_markers"; +constexpr const char* kUpdateFrequency = "update_frequency"; + +namespace nexus_gazebo { + +RigidBodyTracker::RigidBodyTracker(const std::string& _tracker_name, + vrpn_Connection* c) +: vrpn_Tracker(_tracker_name.c_str(), c) +{ +} + +void RigidBodyTracker::mainloop() +{ + this->pos[0] = this->pose.X(); + this->pos[1] = this->pose.Y(); + this->pos[2] = this->pose.Z(); + + this->d_quat[0] = this->pose.Rot().X(); + this->d_quat[1] = this->pose.Rot().Y(); + this->d_quat[2] = this->pose.Rot().Z(); + this->d_quat[3] = this->pose.Rot().W(); + + std::array msgbuf {}; + int len = vrpn_Tracker::encode_to(msgbuf.data()); + + if (!static_cast(this->d_connection->connected())) + return; + + if (0 > this->d_connection->pack_message( + len, + this->timestamp, + position_m_id, + d_sender_id, msgbuf.data(), + vrpn_CONNECTION_LOW_LATENCY)) + { + return; + } + + server_mainloop(); +} + +void RigidBodyTracker::updatePose( + const std::chrono::steady_clock::duration& _sim_time, + const Pose3d& _pose) +{ + this->pose = _pose; + this->timestamp.tv_sec = _sim_time.count() / 1000; + this->timestamp.tv_usec = (_sim_time.count() % 1000) * 1000; +} + +////////////////////////////////////////////////// +MotionCaptureSystem::MotionCaptureSystem() = default; + +////////////////////////////////////////////////// +MotionCaptureSystem::~MotionCaptureSystem() = default; + +////////////////////////////////////////////////// +void MotionCaptureSystem::Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& /*_ecm*/, + EventManager& /*_eventMgr*/) +{ + this->entity = _entity; + + // Read SDF configuration + this->port = _sdf->Get(kPort, this->port).first; + this->stream_rigid_bodies = + _sdf->Get(kSdfStreamRigid, this->stream_rigid_bodies).first; + this->stream_labeled_markers = + _sdf->Get(kSdfStreamLabeled, this->stream_labeled_markers).first; + this->stream_unlabeled_markers = + _sdf->Get(kSdfStreamUnlabeled, this->stream_unlabeled_markers).first; + this->update_frequency = + _sdf->Get(kUpdateFrequency, this->update_frequency).first; + + // Configure the VRPN server + this->connection = vrpn_ConnectionPtr::create_server_connection(this->port); +} + +////////////////////////////////////////////////// +void MotionCaptureSystem::PostUpdate( + const UpdateInfo& _info, + const EntityComponentManager& _ecm) +{ + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time" << std::endl; + } + + if (_info.paused) + { + return; + } + + auto diff = _info.simTime - this->last_pose_pub_time; + if ((diff > std::chrono::steady_clock::duration::zero()) && + (diff < std::chrono::duration {1.0 / this->update_frequency})) + { + return; + } + + // Tracker pose in world frame + const auto pose_WT = gz::sim::worldPose(this->entity, _ecm); + + if (this->stream_rigid_bodies) + { + _ecm.Each( + [&](const Entity& _entity, + const components::MotionCaptureRigidBody* _rigid_body)->bool + { + // Rigid body pose in world frame + const auto pose_WB = gz::sim::worldPose(_entity, _ecm); + + // Rigid body pose in tracker frame (pose_TB = pose_TW * pose_WB) + const auto pose_TB = pose_WT.Inverse() * pose_WB; + + if (this->trackers.count(_entity) == 0) + { + ignmsg << "Creating Tracker: " << _rigid_body->Data() << std::endl; + this->trackers.insert( + { + _entity, + std::make_unique( + _rigid_body->Data(), + this->connection.get()) + }); + } + this->trackers[_entity]->updatePose(_info.simTime, pose_TB); + return true; + }); + } + + for (auto&[_, tracker] : this->trackers) + { + tracker->mainloop(); + } + this->connection->mainloop(); +} + +} // namespace nexus_gazebo + +GZ_ADD_PLUGIN( + nexus_gazebo::MotionCaptureSystem, + nexus_gazebo::System, + nexus_gazebo::MotionCaptureSystem::ISystemConfigure, + nexus_gazebo::MotionCaptureSystem::ISystemPostUpdate) diff --git a/nexus_gazebo/worlds/example.sdf b/nexus_gazebo/worlds/example.sdf new file mode 100644 index 0000000..c24c7ae --- /dev/null +++ b/nexus_gazebo/worlds/example.sdf @@ -0,0 +1,38 @@ + + + + origin_mocap + model://motion_capture_system + 0 0 0 0 0 0 + true + + + + rotated_mocap + model://motion_capture_system + 0 0 0 0 0 1.5707 + true + + + + model://motion_capture_rigid_body + calibration_link_x + 1 0 0 0 0 0 + true + + + + model://motion_capture_rigid_body + calibration_link_y + 0 1 0 0 0 0 + true + + + + model://motion_capture_rigid_body + calibration_link_z + 0 1 0 0 0 0 + true + + + diff --git a/nexus_integration_tests/CMakeLists.txt b/nexus_integration_tests/CMakeLists.txt index 5d941c7..ea52012 100644 --- a/nexus_integration_tests/CMakeLists.txt +++ b/nexus_integration_tests/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(ament_cmake REQUIRED) find_package(nexus_endpoints REQUIRED) find_package(nexus_transporter REQUIRED) find_package(yaml_cpp_vendor REQUIRED) +find_package(yaml-cpp REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) @@ -34,7 +35,7 @@ target_link_libraries(mock_detection PRIVATE nexus_endpoints::nexus_endpoints rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle - ${yaml_cpp_vendor_TARGETS} + yaml-cpp::yaml-cpp ${tf2_ros_TARGETS} ) target_include_directories(mock_detection PRIVATE @@ -138,12 +139,13 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src test - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE C++ - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src test + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) if (ament_cmake_catch2_FOUND) find_package(ament_index_cpp REQUIRED) @@ -163,12 +165,11 @@ if(BUILD_TESTING) rclcpp_action::rclcpp_action rclcpp_lifecycle::rclcpp_lifecycle rcpputils::rcpputils - ${yaml_cpp_vendor_TARGETS} + yaml-cpp::yaml-cpp rmf_utils::rmf_utils ${tf2_ros_TARGETS} ) target_include_directories(test_mocks PUBLIC - ${yaml_cpp_vendor_INCLUDE_DIRS} ${tf2_ros_INCLUDE_DIRS} ) install(TARGETS test_mocks RUNTIME DESTINATION lib/${PROJECT_NAME}) diff --git a/nexus_integration_tests/README.md b/nexus_integration_tests/README.md index 7821e1d..1f5c036 100644 --- a/nexus_integration_tests/README.md +++ b/nexus_integration_tests/README.md @@ -5,7 +5,7 @@ The [launch.py script](launch/launch.py) will launch the system orchestrator and >NOTE: The ROS_DOMAIN_ID occupied by the Zenoh bridges during launch time may be different from the `domain` values in the Zenoh bridge configurations. This is because the launch file overrides the domain ID of the zenoh bridges to ensure that it is same as that of the orchestrator. -### Launch system orchestrator, IRB1300 workcell and IRB910SC Workcell together with Zenoh bridge +### Method 1: Launch system orchestrator, IRB1300 workcell and IRB910SC Workcell together with Zenoh bridge > NOTE: Before running any of these commands, you must set the rmw implmentation to cyclonedds with `export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp` (If testing with real hardware, specify the arguments `use_fake_hardware=False`, `robot1_ip=` and `robot2_ip=`) @@ -13,7 +13,7 @@ The [launch.py script](launch/launch.py) will launch the system orchestrator and ros2 launch nexus_integration_tests launch.py headless:=False ``` -### Launch System Orchestrator and 1 Workcell without Zenoh bridge (Same ROS_DOMAIN_ID) +### Method 2: Launch System Orchestrator and 1 Workcell without Zenoh bridge (Same ROS_DOMAIN_ID) Launch with Workcell 1 ```bash ros2 launch nexus_integration_tests launch.py headless:=False use_zenoh_bridge:=False run_workcell_1:=true run_workcell_2:=false @@ -46,12 +46,8 @@ ros2 launch nexus_integration_tests workcell.launch.py workcell_id:=workcell_1 r ros2 launch nexus_integration_tests workcell.launch.py workcell_id:=workcell_2 ros_domain_id:=2 support_package:=abb_irb1300_support robot_xacro_file:=irb1300_10_115.xacro moveit_config_package:=abb_irb1300_10_115_moveit_config controllers_file:=abb_irb1300_controllers.yaml moveit_config_file:=abb_irb1300_10_115.srdf.xacro tf_publisher_launch_file:=irb1300_tf.launch.py sku_detection_params_file:=irb1300_detection.yaml zenoh_config_file:=workcell_2.json5 headless:=False ``` -### Send a goal to the workcell orchestrator -```bash -ros2 action send_goal /test_workcell/request nexus_orchestrator_msgs/action/WorkcellTask "$(cat config/workcell_task.yaml)" -f -``` +## Submit a job -### Send a work order to the system orchestrator > Note: Set your ROS_DOMAIN_ID environment variable to that of the system orchestrator before executing the work order `place_on_conveyor` work order: @@ -64,6 +60,16 @@ ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {id: '24', work_order: '$(cat config/pick_from_conveyor.json)'}}" ``` +## Debugging + +## workcell + +Send a request to a specific workcell, eg. `test_workcell`. + +```bash +ros2 action send_goal /test_workcell/request nexus_orchestrator_msgs/action/WorkcellTask "$(cat config/workcell_task.yaml)" -f +``` + ## mock gripper and mock detection Gripper position from 0 to `gripper_max_value`: diff --git a/nexus_integration_tests/launch/include/abb_irb1300_moveit.launch.py b/nexus_integration_tests/launch/include/abb_irb1300_moveit.launch.py index 49feb3d..f42a305 100644 --- a/nexus_integration_tests/launch/include/abb_irb1300_moveit.launch.py +++ b/nexus_integration_tests/launch/include/abb_irb1300_moveit.launch.py @@ -81,10 +81,14 @@ def launch_setup(context, *args, **kwargs): "abb_irb1300_10_115_moveit_config", "config/kinematics.yaml" ) + joint_limits_yaml = { + "robot_description_planning": load_yaml("abb_irb1300_10_115_moveit_config", "config/joint_limits.yaml") + } + # Planning Functionality ompl_planning_pipeline_config = { "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", + "planning_plugins": ["ompl_interface/OMPLPlanner"], "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } @@ -99,8 +103,8 @@ def launch_setup(context, *args, **kwargs): "abb_irb1300_10_115_moveit_config", "config/moveit_controllers.yaml" ) moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + "moveit_simple_controller_manager": moveit_simple_controllers_yaml["moveit_simple_controller_manager"], } trajectory_execution = { @@ -127,6 +131,7 @@ def launch_setup(context, *args, **kwargs): robot_description, robot_description_semantic, kinematics_yaml, + joint_limits_yaml, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, diff --git a/nexus_integration_tests/launch/include/abb_irb910sc_moveit.launch.py b/nexus_integration_tests/launch/include/abb_irb910sc_moveit.launch.py index 05f6849..058d8ba 100644 --- a/nexus_integration_tests/launch/include/abb_irb910sc_moveit.launch.py +++ b/nexus_integration_tests/launch/include/abb_irb910sc_moveit.launch.py @@ -82,10 +82,14 @@ def launch_setup(context, *args, **kwargs): "abb_irb910sc_3_45_moveit_config", "config/kinematics.yaml" ) + joint_limits_yaml = { + "robot_description_planning": load_yaml("abb_irb910sc_3_45_moveit_config", "config/joint_limits.yaml") + } + # Planning Functionality ompl_planning_pipeline_config = { "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", + "planning_plugins": ["ompl_interface/OMPLPlanner"], "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } @@ -100,8 +104,8 @@ def launch_setup(context, *args, **kwargs): "abb_irb910sc_3_45_moveit_config", "config/moveit_controllers.yaml" ) moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + "moveit_simple_controller_manager": moveit_simple_controllers_yaml["moveit_simple_controller_manager"], } trajectory_execution = { @@ -128,6 +132,7 @@ def launch_setup(context, *args, **kwargs): robot_description, robot_description_semantic, kinematics_yaml, + joint_limits_yaml, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, diff --git a/nexus_integration_tests/package.xml b/nexus_integration_tests/package.xml index 5b51faa..2cb79d9 100644 --- a/nexus_integration_tests/package.xml +++ b/nexus_integration_tests/package.xml @@ -23,6 +23,7 @@ rclpy std_srvs tf2_ros + yaml-cpp yaml_cpp_vendor rmw_cyclonedds_cpp diff --git a/nexus_integration_tests/src/mock_detection.hpp b/nexus_integration_tests/src/mock_detection.hpp index bf23225..95a8ab9 100644 --- a/nexus_integration_tests/src/mock_detection.hpp +++ b/nexus_integration_tests/src/mock_detection.hpp @@ -47,9 +47,7 @@ class DetectorNode : public rclcpp_lifecycle::LifecycleNode configfile_ = this->declare_parameter("config_file", _configFile); frame_id_ = this->declare_parameter("frame_id", "camera_color_frame"); - bool autostart; - this->declare_parameter("autostart", false); - this->get_parameter("autostart", autostart); + bool autostart = this->declare_parameter("autostart", false); // If 'autostart' parameter is true, the node self-transitions to 'active' state upon startup if (autostart) { diff --git a/nexus_integration_tests/src/mock_gripper.hpp b/nexus_integration_tests/src/mock_gripper.hpp index 4f2c2dd..785e666 100644 --- a/nexus_integration_tests/src/mock_gripper.hpp +++ b/nexus_integration_tests/src/mock_gripper.hpp @@ -46,9 +46,7 @@ class MockGripper : public rclcpp_lifecycle::LifecycleNode increment_ = this->declare_parameter("increment", 0.01); - bool autostart; - this->declare_parameter("autostart", false); - this->get_parameter("autostart", autostart); + bool autostart = this->declare_parameter("autostart", false); // If 'autostart' parameter is true, the node self-transitions to 'active' state upon startup if (autostart) { diff --git a/nexus_integration_tests/src/mock_printer.cpp b/nexus_integration_tests/src/mock_printer.cpp index 768e790..36b2b98 100644 --- a/nexus_integration_tests/src/mock_printer.cpp +++ b/nexus_integration_tests/src/mock_printer.cpp @@ -32,9 +32,7 @@ class MockPrinter : public rclcpp_lifecycle::LifecycleNode "Mock printer is running..." ); - bool autostart; - this->declare_parameter("autostart", false); - this->get_parameter("autostart", autostart); + bool autostart = this->declare_parameter("autostart", false); // If 'autostart' parameter is true, the node self-transitions to 'active' state upon startup if (autostart) { diff --git a/nexus_lifecycle_manager/CMakeLists.txt b/nexus_lifecycle_manager/CMakeLists.txt index 02db020..aa03c06 100644 --- a/nexus_lifecycle_manager/CMakeLists.txt +++ b/nexus_lifecycle_manager/CMakeLists.txt @@ -79,12 +79,13 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src test - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE C++ - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src test + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) find_package(ament_cmake_catch2 QUIET) if (ament_cmake_catch2_FOUND) diff --git a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp index 65dae2f..1c07a7a 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -496,7 +496,7 @@ class LifecycleManager "/" + node_name + std::string("/is_active"), std::bind(&Implementation::isActiveCallback, _pimpl.get(), _1, _2, _3), - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), _pimpl->callback_group_); _pimpl->_manager_srv = n->template create_service( @@ -527,7 +527,7 @@ class LifecycleManager break; } }, - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), _pimpl->callback_group_); } diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt index e3d53a1..539beb6 100644 --- a/nexus_motion_planner/CMakeLists.txt +++ b/nexus_motion_planner/CMakeLists.txt @@ -94,16 +94,19 @@ if(BUILD_TESTING) RUNTIME DESTINATION lib/${PROJECT_NAME} ) - ament_uncrustify( - ARGN include src test - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE CPP - ) - - ament_add_pytest_test(test_motion_plan_cache_py "test/test_motion_plan_cache.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src test + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) + + # TODO(luca) reintroduce this test, it was failing since the beginning so unclear + # what it it supposed to do + #ament_add_pytest_test(test_motion_plan_cache_py "test/test_motion_plan_cache.py" + # WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + #) # Motion planner server test with cache mode unset add_launch_test( diff --git a/nexus_motion_planner/launch/demo_planner_server.launch.py b/nexus_motion_planner/launch/demo_planner_server.launch.py index ebf13e4..79d8a40 100644 --- a/nexus_motion_planner/launch/demo_planner_server.launch.py +++ b/nexus_motion_planner/launch/demo_planner_server.launch.py @@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs): # Planning Functionality ompl_planning_pipeline_config = { "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", + "planning_plugins": ["ompl_interface/OMPLPlanner"], "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } @@ -111,7 +111,7 @@ def launch_setup(context, *args, **kwargs): moveit_config_package.perform(context), "config/moveit_controllers.yaml" ) moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_simple_controller_manager": moveit_simple_controllers_yaml["moveit_simple_controller_manager"], "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } diff --git a/nexus_motion_planner/test/test_request.test.py b/nexus_motion_planner/test/test_request.test.py index a4ab8a4..ab1c51f 100644 --- a/nexus_motion_planner/test/test_request.test.py +++ b/nexus_motion_planner/test/test_request.test.py @@ -150,10 +150,14 @@ def generate_test_description(): kinematics_yaml = load_yaml( moveit_config_package, "config/kinematics.yaml") + joint_limits_yaml = { + "robot_description_planning": load_yaml(moveit_config_package, "config/joint_limits.yaml") + } + # Planning Functionality ompl_planning_pipeline_config = { "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", + "planning_plugins": ["ompl_interface/OMPLPlanner"], "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } @@ -224,6 +228,7 @@ def generate_test_description(): robot_description, robot_description_semantic, kinematics_yaml, + joint_limits_yaml, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, diff --git a/nexus_motion_planner/test/test_request_with_cache.test.py b/nexus_motion_planner/test/test_request_with_cache.test.py index ec92c44..98f31fd 100644 --- a/nexus_motion_planner/test/test_request_with_cache.test.py +++ b/nexus_motion_planner/test/test_request_with_cache.test.py @@ -150,10 +150,14 @@ def generate_test_description(): kinematics_yaml = load_yaml( moveit_config_package, "config/kinematics.yaml") + joint_limits_yaml = { + "robot_description_planning": load_yaml(moveit_config_package, "config/joint_limits.yaml") + } + # Planning Functionality ompl_planning_pipeline_config = { "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", + "planning_plugins": ["ompl_interface/OMPLPlanner"], "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } @@ -224,6 +228,7 @@ def generate_test_description(): robot_description, robot_description_semantic, kinematics_yaml, + joint_limits_yaml, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, diff --git a/nexus_network_configuration/nexus_network_configuration/nexus_network_configuration.py b/nexus_network_configuration/nexus_network_configuration/nexus_network_configuration.py index e49a93a..d97de9e 100644 --- a/nexus_network_configuration/nexus_network_configuration/nexus_network_configuration.py +++ b/nexus_network_configuration/nexus_network_configuration/nexus_network_configuration.py @@ -216,6 +216,8 @@ def generate_zenoh_config(self, output_dir): + "." + self.zenoh_cfg_file_extension, ) + # This has been removed in Zenoh 1.0.0 + del zenoh_cfg["plugins"]["dds"]["group_member_id"] self.write_to_json(write_filepath, zenoh_cfg) print(f"Generated Zenoh configuration at {write_filepath}") diff --git a/nexus_network_configuration/scripts/set_up_network.sh b/nexus_network_configuration/scripts/set_up_network.sh index 123f353..76e64a6 100755 --- a/nexus_network_configuration/scripts/set_up_network.sh +++ b/nexus_network_configuration/scripts/set_up_network.sh @@ -5,7 +5,7 @@ # 2. Change RMW_IMPLEMENTATION to CycloneDDS # 3. Enable multicast on loopback interface -RMW_PACKAGE="ros-iron-rmw-cyclonedds-cpp" +RMW_PACKAGE="ros-jazzy-rmw-cyclonedds-cpp" PKG_OK=$(dpkg-query -W --showformat='${Status}\n' $RMW_PACKAGE|grep "install ok installed") diff --git a/nexus_robot_controller/CMakeLists.txt b/nexus_robot_controller/CMakeLists.txt index 7278130..ede0773 100644 --- a/nexus_robot_controller/CMakeLists.txt +++ b/nexus_robot_controller/CMakeLists.txt @@ -53,12 +53,13 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE CPP - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) find_package(launch_testing_ament_cmake) install(DIRECTORY test DESTINATION share/${PROJECT_NAME}) diff --git a/nexus_robot_controller/src/robot_controller_server.cpp b/nexus_robot_controller/src/robot_controller_server.cpp index 3ca42eb..05f02c5 100644 --- a/nexus_robot_controller/src/robot_controller_server.cpp +++ b/nexus_robot_controller/src/robot_controller_server.cpp @@ -249,7 +249,7 @@ RobotControllerServer::on_activate(const rclcpp_lifecycle::State& /*state*/) std::vector controller_names; for (auto controller : pimpl_->cm_node_->get_loaded_controllers()) { - if (controller.c->get_state().label() != ACTIVE) + if (controller.c->get_lifecycle_state().label() != ACTIVE) { RCLCPP_INFO_STREAM( get_logger(), @@ -277,7 +277,7 @@ RobotControllerServer::on_deactivate(const rclcpp_lifecycle::State& /*state*/) std::vector controller_names; for (auto controller : pimpl_->cm_node_->get_loaded_controllers()) { - if (controller.c->get_state().label() == ACTIVE) + if (controller.c->get_lifecycle_state().label() == ACTIVE) { RCLCPP_INFO_STREAM( get_logger(), diff --git a/nexus_rviz_plugins/CMakeLists.txt b/nexus_rviz_plugins/CMakeLists.txt index a3c2a71..6b4b627 100644 --- a/nexus_rviz_plugins/CMakeLists.txt +++ b/nexus_rviz_plugins/CMakeLists.txt @@ -74,12 +74,13 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src test - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE C++ - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src test + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) endif() ament_package() diff --git a/nexus_system_orchestrator/CMakeLists.txt b/nexus_system_orchestrator/CMakeLists.txt index 98fffa8..d3459bb 100644 --- a/nexus_system_orchestrator/CMakeLists.txt +++ b/nexus_system_orchestrator/CMakeLists.txt @@ -12,7 +12,8 @@ find_package(nexus_cmake REQUIRED) find_package(nexus_common REQUIRED) find_package(nexus_endpoints REQUIRED) find_package(nexus_lifecycle_manager REQUIRED) -find_package(yaml-cpp REQUIRED VERSION 0.7) +find_package(yaml_cpp_vendor REQUIRED) +find_package(yaml-cpp REQUIRED) set(CMAKE_CXX_FLAGS "-Wall -Wpedantic") @@ -37,7 +38,7 @@ target_link_libraries(${PROJECT_NAME}_plugin PUBLIC rclcpp_action::rclcpp_action rclcpp_lifecycle::rclcpp_lifecycle rclcpp_components::component - yaml-cpp + yaml-cpp::yaml-cpp ) set_target_properties(${PROJECT_NAME}_plugin PROPERTIES CXX_VISIBILITY_PRESET hidden) target_compile_definitions(${PROJECT_NAME}_plugin PRIVATE "COMPOSITION_BUILDING_DLL") @@ -57,12 +58,13 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE CPP - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) # Adds a ament catch2 test with some common libraries. # Usage: nexus_add_test( ...) diff --git a/nexus_system_orchestrator/package.xml b/nexus_system_orchestrator/package.xml index 2b1a2f4..6aa3829 100644 --- a/nexus_system_orchestrator/package.xml +++ b/nexus_system_orchestrator/package.xml @@ -13,6 +13,7 @@ behaviortree_cpp_v3 nexus_lifecycle_manager yaml-cpp + yaml_cpp_vendor nexus_cmake nexus_common diff --git a/nexus_transporter/CMakeLists.txt b/nexus_transporter/CMakeLists.txt index 3e9dd3f..19f5240 100644 --- a/nexus_transporter/CMakeLists.txt +++ b/nexus_transporter/CMakeLists.txt @@ -100,11 +100,13 @@ if(BUILD_TESTING AND ament_cmake_uncrustify_FOUND AND ament_cmake_catch2_FOUND) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src test - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src test + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) ament_add_catch2(test_nexus_transporter test/main.cpp diff --git a/nexus_transporter/src/TransporterNode.cpp b/nexus_transporter/src/TransporterNode.cpp index 4cfed65..6cc3074 100644 --- a/nexus_transporter/src/TransporterNode.cpp +++ b/nexus_transporter/src/TransporterNode.cpp @@ -110,7 +110,7 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) response->transporter = itinerary->transporter_name(); response->estimated_finish_time = itinerary->estimated_finish_time(); }, - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), _data->cb_group ); @@ -464,7 +464,7 @@ bool TransporterNode::registration_callback() RCLCPP_INFO(this->get_logger(), "Registering with system orchestrator..."); auto client = this->create_client( RegisterTransporterService::service_name(), - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), _data->cb_group ); 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": {} +} diff --git a/nexus_workcell_orchestrator/CMakeLists.txt b/nexus_workcell_orchestrator/CMakeLists.txt index 57caf7c..ecd68a8 100644 --- a/nexus_workcell_orchestrator/CMakeLists.txt +++ b/nexus_workcell_orchestrator/CMakeLists.txt @@ -19,7 +19,8 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(vision_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(yaml-cpp REQUIRED VERSION 0.7) +find_package(yaml_cpp_vendor REQUIRED) +find_package(yaml-cpp REQUIRED) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -59,7 +60,7 @@ target_link_libraries(${PROJECT_NAME}_plugin PUBLIC tf2::tf2 tf2_geometry_msgs::tf2_geometry_msgs tf2_ros::tf2_ros - yaml-cpp + yaml-cpp::yaml-cpp ${vision_msgs_TARGETS} ${geometry_msgs_TARGETS} ) @@ -82,12 +83,13 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") - ament_uncrustify( - ARGN include src - CONFIG_FILE ${uncrustify_config_file} - MAX_LINE_LENGTH 80 - LANGUAGE CPP - ) + # TODO(luca) Add uncrustify back after formatting + #ament_uncrustify( + # ARGN include src + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + # LANGUAGE CPP + #) # Adds a ament catch2 test with some common libraries. # Usage: nexus_add_test( ...) diff --git a/nexus_workcell_orchestrator/package.xml b/nexus_workcell_orchestrator/package.xml index a33d751..bcf46ec 100644 --- a/nexus_workcell_orchestrator/package.xml +++ b/nexus_workcell_orchestrator/package.xml @@ -13,6 +13,7 @@ nexus_common nexus_endpoints rclcpp_components + rmf_utils behaviortree_cpp_v3 geometry_msgs @@ -27,9 +28,9 @@ tf2_ros vision_msgs yaml-cpp + yaml_cpp_vendor ament_cmake_uncrustify - rmf_utils ament_cmake diff --git a/nexus_workcell_orchestrator/src/main_test.cpp b/nexus_workcell_orchestrator/src/main_test.cpp index 08ed3f5..effee9f 100644 --- a/nexus_workcell_orchestrator/src/main_test.cpp +++ b/nexus_workcell_orchestrator/src/main_test.cpp @@ -16,4 +16,4 @@ */ #define CATCH_CONFIG_MAIN -#include +#include diff --git a/nexus_workcell_orchestrator/src/serialization_test.cpp b/nexus_workcell_orchestrator/src/serialization_test.cpp index fd6ec66..2eab72e 100644 --- a/nexus_workcell_orchestrator/src/serialization_test.cpp +++ b/nexus_workcell_orchestrator/src/serialization_test.cpp @@ -24,7 +24,7 @@ #include #include -#include +#include namespace nexus::workcell_orchestrator::test { diff --git a/nexus_workcell_orchestrator/src/task_results_test.cpp b/nexus_workcell_orchestrator/src/task_results_test.cpp index 31f5e54..51503a8 100644 --- a/nexus_workcell_orchestrator/src/task_results_test.cpp +++ b/nexus_workcell_orchestrator/src/task_results_test.cpp @@ -27,7 +27,7 @@ #include #include -#include +#include namespace nexus::workcell_orchestrator::test { diff --git a/nexus_workcell_orchestrator/src/transform_pose_test.cpp b/nexus_workcell_orchestrator/src/transform_pose_test.cpp index 2d688ed..ae40807 100644 --- a/nexus_workcell_orchestrator/src/transform_pose_test.cpp +++ b/nexus_workcell_orchestrator/src/transform_pose_test.cpp @@ -15,7 +15,8 @@ * */ -#include +#define CATCH_CONFIG_MAIN +#include #include "transform_pose.hpp" diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp index 6917a42..5092968 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp @@ -667,7 +667,8 @@ void WorkcellOrchestrator::_register() RCLCPP_INFO(this->get_logger(), "Successfully registered with system orchestrator"); this->_register_timer->cancel(); - this->_register_timer.reset(); + // TODO(luca) reintroduce once https://github.com/ros2/rclcpp/issues/2652 is fixed and released + // this->_register_timer.reset(); }; if (!this->_register_workcell_client->wait_for_service( diff --git a/nexus_zenoh_bridge_dds_vendor/CMakeLists.txt b/nexus_zenoh_bridge_dds_vendor/CMakeLists.txt index 4cd13dc..ea92efd 100644 --- a/nexus_zenoh_bridge_dds_vendor/CMakeLists.txt +++ b/nexus_zenoh_bridge_dds_vendor/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(ament_cmake_vendor_package REQUIRED) ament_vendor(zeno_bridge_dds_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-plugin-dds.git - VCS_VERSION 0.7.2-rc + VCS_VERSION 1.0.2 ) # TODO(sloretz) make a nice way to get this path from ament_vendor diff --git a/nexus_zenoh_bridge_dds_vendor/package.xml b/nexus_zenoh_bridge_dds_vendor/package.xml index fa6cfe8..79bb1bc 100644 --- a/nexus_zenoh_bridge_dds_vendor/package.xml +++ b/nexus_zenoh_bridge_dds_vendor/package.xml @@ -10,12 +10,7 @@ ament_cmake ament_cmake_vendor_package - - + cargo clang