calibration-movement-framos.py

#!/usr/bin/env python

import rospy
import moveit_commander
import time
import tf.transformations
import tf, tf2_ros, tf2_geometry_msgs
import yaml, json
import criutils as cu
from geometry_msgs.msg import Pose
from handeye.srv import CalibrateHandEye, CalibrateHandEyeRequest

def move_arm_to_pose(move_group, pose):
    if rospy.is_shutdown():
        return

    move_group.set_pose_target(pose)
    print(pose)
    (response, plan, planning_time, error_code) = move_group.plan()
    if not response:
        rospy.logerr(f"Failed to plan to pose: {pose}")
        rospy.shutdown()
        return

    print(plan)
    move_group.execute(plan, wait=True)

def move_arm_to_joint_positions(move_group, joint_positions):
    if rospy.is_shutdown():
        return

    move_group.set_joint_value_target(joint_positions)
    print(f"move_arm_to_joint_positions - Setting target to: {joint_positions}")
    (response, plan, planning_time, error_code) = move_group.plan()
    if not response:
        rospy.logerr(f"Failed to plan to joint positions: {joint_positions}")
        rospy.shutdown()
        return

    #print(plan)
    move_group.execute(plan, wait=True)

def get_transform_as_pose(tf_buffer, from_frame, to_frame):
    try:
        transform = tf_buffer.lookup_transform(from_frame, to_frame, rospy.Time(0), rospy.Duration(1.0))
        if transform is None:
            return None

        pose = Pose()
        pose.position.x = transform.transform.translation.x
        pose.position.y = transform.transform.translation.y
        pose.position.z = transform.transform.translation.z
        pose.orientation.x = transform.transform.rotation.x
        pose.orientation.y = transform.transform.rotation.y
        pose.orientation.z = transform.transform.rotation.z
        pose.orientation.w = transform.transform.rotation.w
        print(f"The position between ({from_frame} and {to_frame}) is: {pose.position}")
    except tf.ExtrapolationException as e:
        rospy.logerr(f'Extrapolation exception received: {e}')
        return None
    return pose

def msg_to_json(msg):
    value = yaml.safe_load(str(msg))
    return json.dumps(value, indent=4)

def msg_to_yaml(msg):
    return yaml.dump(msg_to_json(msg))

def main():
    # Initialise
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('calibration-movement-framos-node')
    move_group = moveit_commander.MoveGroupCommander("manipulator")

    srv_name = '/handeye_calibration'
    rospy.wait_for_service(srv_name)
    calibrate_srv = rospy.ServiceProxy(srv_name, CalibrateHandEye)

    # WARNING - Test Poses
    # joint_names:
    #   - shoulder_pan_joint
    #   - shoulder_lift_joint
    #   - elbow_joint
    #   - wrist_1_joint
    #   - wrist_2_joint
    #   - wrist_3_joint
    # stow_pose:
    #   [0.0, -2.125, 2,486, 4.306, -1.568, 0.0]
    joint_stow_position = [0.0, -2.125, 2.486, 4.306, -1.568, 0.0]
    print(f"Current joint values are....{move_group.get_current_joint_values()}")
    print(f"Current MoveIt: {move_group.get_current_pose()}")
    print(f"Current RPY: {move_group.get_current_rpy()}")

    # Read in the calibration joint positions
    joint_positions = []
    with open('./calibration/20-joint-states.yaml', newline='') as the_file:
        # yaml file as dictionary
        joint_state_dict = yaml.safe_load(the_file)

        #print(f"Calibration joint positions are....{joint_state_dict}")
        joint_values = joint_state_dict['joint_values']
        #print(f"Calibration joint values are....{joint_values}")
        print(f"Number of joint values read: {len(joint_values)}")

    tf_buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tf_buffer)
    tf_publisher = tf.TransformBroadcaster()

    # Move through the calibration poses
    try:
        while not rospy.is_shutdown():
            move_arm_to_joint_positions(move_group, joint_stow_position)
            print(f"Starting at the stow location: {joint_stow_position}")
            time.sleep(1)

            request = CalibrateHandEyeRequest()
            request.setup = 'Moving'
            #solver = 'ParkBryan1994'
            #solver = 'TsaiLenz1989'
            solver = 'Daniilidis1999'
            request.solver = solver

            for joint_position in joint_values:
                print(f"Moving to joint position: {joint_position}\n")
                move_arm_to_joint_positions(move_group, joint_position)
                time.sleep(1)
                object_pose = None
                effector_pose = None

                #request.effector_wrt_world.poses.append(move_group.get_current_pose().pose)
                # get transform from camera to handeye_target
                try:
                    object_pose = get_transform_as_pose(tf_buffer, 'camera_color_optical_frame', 'handeye_target')
                    effector_pose = get_transform_as_pose(tf_buffer, 'base_link', 'tool0')
                except tf.ExtrapolationException as e:
                    rospy.logerr(f'Extrapolation exception received: {e}')
                    # this pose has no corresponding object pose
                    continue
                
                if object_pose is None or effector_pose is None:
                    continue

                request.effector_wrt_world.poses.append(effector_pose)
                request.object_wrt_sensor.poses.append(object_pose)

                try:
                    response = calibrate_srv(request)
                    print(f"Calibration service using {solver} returned: {response}")

                    if response.success == False:
                        rospy.logerr("Calibration failed")
                        continue

                    eff_to_sensor = response.sensor_frame
                    print(f"\n---\n")
                    print(f"Calibrated estimate of effector to sensor: {eff_to_sensor}")
                    print(f"\n---\n")
                    # publish transform from effector to sensor
                    tf_publisher.sendTransform((eff_to_sensor.position.x, eff_to_sensor.position.y, eff_to_sensor.position.z),
                                     (eff_to_sensor.orientation.x, eff_to_sensor.orientation.y, eff_to_sensor.orientation.z, eff_to_sensor.orientation.w),
                                     rospy.Time.now(),
                                     "camera_color_optical_frame",
                                     "tool0")
                except rospy.ServiceException as e:
                    rospy.logerr(f"Service call failed: {e}")
                    continue

            move_arm_to_joint_positions(move_group, joint_stow_position)
            print(f"Starting at the stow location: {joint_stow_position}")
            time.sleep(1)

            # Write the calibration results to a the_file
            current_time = time.strftime("%d_%m_%Y-%H_%M_%S")

            with open(f'/home/qcr/cgras2025_ws/src/scripts/calibration/service-calibration-results-{solver}-{current_time}.json', 'w') as the_file:
                #json.dump(msg_to_json(request), the_file)
                #json.dump(msg_to_json(response), the_file)
                #yaml.dump(msg_to_json(request), the_file)
                #yaml.dump(msg_to_json(response), the_file)
                yaml.dump(request, the_file)
                yaml.dump(response, the_file)
            break

    except KeyboardInterrupt:
        print("Interrupted by user")

    # Shut down MoveIt cleanly
    moveit_commander.roscpp_shutdown()

if __name__ == '__main__':
    import sys
    main()