def main(): rospy.init_node('UpdateURDF') dashboard_client = DashboardClient() fixed_bb_offset_path = os.path.join(roslib.packages.get_pkg_dir("arm_fiducial_cal"), "calib/fixed_offset.txt") print 'Reading %s' % fixed_bb_offset_path with open(fixed_bb_offset_path) as f: x, y, z, qw, qx, qy, qz = [float(x) for x in f.readline().split()] print x, y, z, qw, qx, qy, qz bb_left_neck_tf = Transform() bb_left_neck_tf.translation.x = x bb_left_neck_tf.translation.y = y bb_left_neck_tf.translation.z = z bb_left_neck_tf.rotation.w = qw bb_left_neck_tf.rotation.x = qx bb_left_neck_tf.rotation.y = qy bb_left_neck_tf.rotation.z = qz # write out the resulting cal file to the urdf urdf_path = os.path.join(roslib.packages.get_pkg_dir("arm_robot_model"), "models/sensorsValues.urdf.xacro") print 'Updating URDF in %s' % urdf_path update_sensors_values_urdf(urdf_path, bb_left_neck_tf, None, None, None)
opt = FCOptimizer(frames, params.tf_target_points, est_base_H_target, est_cam_H_neck, fix_target_transform=True, W=np.diag([1.0, 1.0, 10.0])) new_est_base_H_target, new_est_cam_H_neck = opt.optimize() opt.print_stats() print np.dot(linalg.inv(new_est_cam_H_neck), est_cam_H_neck) true_color = (1.0, 0.0, 1.0, 1.0) initial_color = (0.0, 1.0, 1.0, 1.0) estimated_color = (1.0, 1.0, 0.0, 1.0) fcviz.draw_frames(upright_frames, est_cam_H_neck, 'new_initial_camera_poses', initial_color, cam_mark_lines=False) fcviz.draw_target(est_base_H_target, 'estimated_target_pose', true_color) fcviz.draw_frames(upright_frames, new_est_cam_H_neck, 'new_estimated_head_poses', estimated_color, cam_mark_lines=False) # write out the resulting cal file to a urdf urdf_path = os.path.join( roslib.packages.get_pkg_dir("arm_robot_model"), "models/sensorsValues.urdf.xacro") print 'Cal Successful! Writing result to URDF to %s' % urdf_path bb_left_neck_tf = ros_util.matrix_to_transform(linalg.inv(new_est_cam_H_neck)) update_sensors_values_urdf(urdf_path, bb_left_neck_tf, None, None, None) while not rospy.is_shutdown(): fcviz.update() rospy.sleep(0.1)