def __init__(self):
# Read parameters
self.ref_frame = cu.read_parameter('~ref_frame', 'torso_base_link')
self.ee_link = cu.read_parameter('~ee_link', 'arm_left_link_tool0')
self.marker_frame = cu.read_parameter('~marker_frame', 'marker_582')
self.cam_frame = cu.read_parameter('~cam_frame',
'camera_optical_frame')
# Initialize the poses msgs
self.ee_samples = []
self.marker_samples = []
# Configure TF listener
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# Use the current time for the bag path
now = time.time()
time_str = datetime.fromtimestamp(now).strftime('%Y-%m-%d-%H-%M-%S')
self.path = '~/.ros/{}-poses.bag'.format(time_str)
# Setup service server
self.save_srv = rospy.Service('/calibration/save', Trigger,
self.cb_save)
self.capture_srv = rospy.Service('/calibration/capture', Trigger,
self.cb_capture)
# Report
srv_name = self.capture_srv.resolved_name
rospy.loginfo('Service is up and running: {}'.format(srv_name))
srv_name = self.save_srv.resolved_name
rospy.loginfo('Service is up and running: {}'.format(srv_name))
def test_read_parameter(self):
# Try the global case
value = cu.read_parameter('~test', 100)
self.assertEqual(value, 100)
# Try the nested case
value = cu.parameter.read_parameter('~test', 100)
self.assertEqual(value, 100)
import os
import yaml
import rospy
import tf2_ros
import numpy as np
import baldor as br
import criutils as cu
from geometry_msgs.msg import TransformStamped
if __name__ == '__main__':
node_name = os.path.splitext(os.path.basename(__file__))[0]
rospy.init_node(node_name)
logger = rospy
logger.loginfo('Starting [%s] node' % node_name)
# Read publish rate
publish_rate = cu.read_parameter('~publish_rate', 60.0)
invert = cu.read_parameter('~invert', False)
# Read all the other parameters
try:
params = rospy.get_param('~')
except KeyError:
logger.logwarn('No parameters found on the parameter server')
exit(0)
expected_keys = ['parent', 'child', 'rotation', 'translation']
params_list = []
for key, data in params.items():
if cu.misc.has_keys(data, expected_keys):
params_list.append(data)
if len(params_list) == 0:
logger.logwarn('No transformations found on the parameter server')
exit(0)
required=True,
help='Path to the rosbag with the calibration poses')
parser.add_argument('--debug',
action='store_true',
help='If set, will show debugging messages')
args = parser.parse_args(clean_argv)
return args
if __name__ == '__main__':
args = parse_args()
log_level = rospy.DEBUG if args.debug else rospy.INFO
node_name = os.path.splitext(os.path.basename(__file__))[0]
rospy.init_node(node_name, log_level=log_level)
# Read parameters
setup = cu.read_parameter('~setup', 'Fixed')
solver = cu.read_parameter('~solver', 'ParkBryan1994')
ref_frame = cu.read_parameter('~ref_frame', 'torso_base_link')
cam_frame = cu.read_parameter('~cam_frame', 'camera_optical_frame')
# Connect to the handeye calibration service
handeye_srv = rospy.ServiceProxy('handeye_calibration', CalibrateHandEye)
try:
handeye_srv.wait_for_service(timeout=2.0)
except rospy.ROSException:
srv_name = handeye_srv.resolved_name
rospy.logerr('Failed to connect to service: {0}'.format(srv_name))
# Process rosbag
request = CalibrateHandEyeRequest()
request.setup = setup
request.solver = solver
bag = rosbag.Bag(os.path.expanduser(args.bag_path))