diff --git a/README.md b/README.md index 731184a9..3b9106c4 100644 --- a/README.md +++ b/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 diff --git a/robot_calibration/CMakeLists.txt b/robot_calibration/CMakeLists.txt index 6eb02458..a81ea1dd 100644 --- a/robot_calibration/CMakeLists.txt +++ b/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) diff --git a/robot_calibration/scripts/capture_poses b/robot_calibration/scripts/capture_poses index 46c6e1bd..a62457fb 100755 --- a/robot_calibration/scripts/capture_poses +++ b/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 ') - 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 ') + 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()