port capture_poses script to ROS 2, document (#181)

README.md

@@ -46,6 +46,25 @@ kinematic chain, for instance, a pan/tilt head).
 Configuration is typically handled through two sets of YAML files: usually
 called ``capture.yaml`` and ``calibrate.yaml``.
 
+If you want to manually move the robot to poses and capture each time you
+hit ENTER on the keyboard, you can run robot calibration with:
+
+```
+ros2 run robot_calibration calibrate --manual --ros-args --params-file path-to-capture.yaml --params-file path-to-calibrate.yaml
+```
+
+More commonly, you will generate a third YAML file with the capture pose
+configuration (as documented below in the section "Calibration Poses"):
+
+```
+ros2 run robot_calibration calibrate path-to-calibration-poses.yaml --ros-args --params-file path-to-capture.yaml --params-file path-to-calibrate.yaml
+```
+
+This is often wrapped into a ROS 2 launch file, which often records
+a bagfile of the observations allowing to re-run just the calibration part
+instead of needing to run capture each time. For an example, see the
+UBR-1 example in the next section.
+
 ### Example Configuration
 
 All of the parameters that can be defined in the capture and calibrate steps
@@ -309,6 +328,54 @@ For each error block, the ``type`` must be specified. In addition to the
     * `rotation_scale` - If `param` is a free frame, multiply the angular distance
       of the free parameter value by this scalar.
 
+### Calibration Poses
+
+The final piece of configuration is the actual poses from which the robot should
+capture data. This YAML file can be created by running the `capture_poses` script.
+You will be prompted to move the robot to the desired pose and press ENTER, when
+done collecting all of your poses, you can type EXIT.
+This will create `calibration_poses.yaml` which is an array of capture poses:
+
+```yaml
+- features: []
+  joints:
+  - first_joint
+  - second_joint
+  positions:
+  - -0.09211555123329163
+  - 0.013307283632457256
+- features: []
+  joints:
+  - first_joint
+  - second_joint
+  positions:
+  - -1.747204065322876
+  - -0.07186950743198395
+```
+
+By default, every finder is used for every capture pose. In some cases, you might
+want to specify specific finders by editing the `features`:
+
+```yaml
+# This sample pose uses only the `ground_plane_finder` feature finder
+- features:
+  - ground_plane_finder
+  joints:
+  - first_joint
+  - second_joint
+  positions:
+  - -0.09211555123329163
+  - 0.013307283632457256
+# This sample pose will use all features
+- features: []
+  joints:
+  - first_joint
+  - second_joint
+  positions:
+  - -1.747204065322876
+  - -0.07186950743198395
+```
+
 #### Checkerboard Configuration
 
 When using a checkerboard, we need to estimate the transformation from the

robot_calibration/CMakeLists.txt

@@ -178,6 +178,12 @@ install(TARGETS
   RUNTIME DESTINATION lib/robot_calibration
 )
 
+install(
+  PROGRAMS
+    scripts/capture_poses
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 pluginlib_export_plugin_description_file(robot_calibration robot_calibration.xml)
 
 ament_export_include_directories(include)

robot_calibration/scripts/capture_poses

@@ -1,5 +1,6 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
+# Copyright (C) 2024 Michael Ferguson
 # Copyright (C) 2015 Fetch Robotics Inc.
 # Copyright (C) 2013-2014 Unbounded Robotics Inc.
 #
@@ -17,46 +18,51 @@
 
 # Author: Michael Ferguson
 
-from __future__ import print_function
+import threading
+import yaml
 
-import string
-
-import rospy
-import rosbag
+import rclpy
+from rclpy.node import Node
 
 from sensor_msgs.msg import JointState
 from robot_calibration_msgs.msg import CaptureConfig
 
 
-class CapturePoses:
+class CapturePoses(Node):
     last_state_ = None # last joint states
 
     def __init__(self):
+        super().__init__('capture_calibration_poses')
         self.last_state_ = CaptureConfig()
+        self.poses = list()
 
-        rospy.init_node('capture_calibration_poses')
-        rospy.Subscriber("joint_states", JointState, self.stateCb)
-
-        # bag to write data to
-        bag = rosbag.Bag('calibration_poses.bag', 'w')
+        self.state_sub = self.create_subscription(JointState, "joint_states", self.stateCb, 1)
 
-        # put samples in
+    def collect(self):
+        # Collect samples
         count = 0
-        while not rospy.is_shutdown():
-            print('Move arm/head to a new sample position:  (type "exit" to quit and save data)')
-            resp = raw_input('press <enter>')
-            if string.upper(resp) == 'EXIT':
-                break
+        while rclpy.ok():
+            print('Move robot to a new sample position:  (type "exit" to quit and save data)')
+            resp = input('press <enter>')
+            if resp.upper() == 'EXIT':
+                print('Exiting...')
+                return
             else:
                 if self.last_state_ == None:
                     print('Cannot save state')
                     continue
-                # save a pose
-                print(self.last_state_)
-                bag.write('calibration_joint_states', self.last_state_)
+                # Save a pose
+                pose = {'features': list(self.last_state_.features),
+                        'joints': list(self.last_state_.joint_states.name),
+                        'positions': list(self.last_state_.joint_states.position)}
+                self.poses.append(pose)
                 print("Saving pose %d" % count)
                 count += 1
-        bag.close()
+
+    def save(self):
+        # Write data to YAML
+        with open('calibration_poses.yaml', 'w') as f:
+            yaml.dump(self.poses, f)
 
     def stateCb(self, msg):
         """ Callback for joint_states messages """
@@ -69,4 +75,13 @@ class CapturePoses:
                 self.last_state_.joint_states.position.append(position)
 
 if __name__=='__main__':
-    CapturePoses()
+    rclpy.init()
+    try:
+        node = CapturePoses()
+        thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+        thread.start()
+        node.collect()
+    finally:
+        node.save()
+        node.destroy_node()
+        rclpy.shutdown()