nexus_calibration (#34)

* Add nexus_calibration package Signed-off-by: Yadunund <[email protected]> * Add to ci Signed-off-by: Yadunund <[email protected]> --------- Signed-off-by: Yadunund <[email protected]>
osrf · Aug 11, 2024 · d8715f0 · d8715f0
1 parent 710dfb1
commit d8715f0
Show file tree

Hide file tree

Showing 11 changed files with 724 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nexus_calibration</name>
+  <version>0.1.1</version>
+  <description>A package with ROS 2 nodes for calibration workcell components</description>
+  <maintainer email="[email protected]">Yadunund</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>nexus_endpoints</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>tf2</depend>
+  <depend>vrpn</depend>
+
+  <test_depend>nexus_gazebo</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>