diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index 0629357..2c294c7 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.github/workflows/nexus_integration_tests.yaml @@ -40,7 +40,7 @@ jobs: rosdep update rosdep install --from-paths . -yir - name: build - run: /ros_entrypoint.sh colcon build --packages-up-to nexus_gazebo 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/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 + + +