calibration-navigation.py

#!/usr/bin/env python

import rospy
import moveit_commander
import time
import tf.transformations
import cmd
import sys
import os
import yaml
import tf, tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import Pose, PoseStamped
import numpy as np
from scipy.spatial.transform import Rotation as R
import actionlib
from cgras_robot_manipulator.msg import MoveAction, MoveResult, MoveActionGoal, MoveGoal
from moveit_calibration_detector.srv import TargetDimension
from handeye.srv import CalibrateHandEye, CalibrateHandEyeRequest

class NavigateTools(cmd.Cmd):
    prompt = "Navigation: pose, joint, relative, tile or goto > "

    def emptyline(self):
        if len(self.captured_joint_values) > 0:
            confirmation = input("WARNING - You have stored joint values. Are you sure you want to exit (y/n)? ")
            if confirmation == 'y':
                self.captured_joint_values = []
            else:
                return False

        print("Exiting...")
        return True

    def do_store(self, arg):
        print("Storing the current joint state...")
        self.init_move_group()

        joint_positions = self.move_group.get_current_joint_values()
        self.captured_joint_values.append(joint_positions)
        print(f"==== {len(self.captured_joint_values)} states currently stored ====")
        print(f"Current joint values: {joint_positions}")
        print("====")

    def do_write(self, arg):
        print("Writing the stored joint configurations...")
        if len(self.captured_joint_values) == 0:
            print("No joint values stored")
            return

        if arg != '':
            current_time = time.strftime("%Y%m%d-%H%M%S")
            file_name = arg.split('.')[0] + f'-joint-states-captured-{current_time}.yaml'
        else:
            file_name = input("Enter the file name to save: ")

        if file_name == '':
            print("Aborting due to empty file name")
            return

        print(f"Saving {len(self.captured_joint_values)} stored joint states to {file_name}")
        # Just writing these (names) to conform to the existing format...
        joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
        print(self.captured_joint_values)
        with open(f'./calibration/{file_name}', 'w') as the_file:
            yaml.dump({'joint_names': joint_names, 'joint_values': self.captured_joint_values}, the_file)

        self.captured_joint_values = []


    def do_pose(self, arg):
        print("POSE based navigation...")
        self.init_move_group()

        pose = Pose()
        if self.sim == False:
            pose = self.move_group.get_current_pose().pose

            if pose is None:
                print("Current pose is None, cannot proceed with relative navigation")
                return

            # Setting default reference frame
            if self.reference_frame is None:
                self.reference_frame = '/robot_footprint' # Assumed standard for robot
                print(f"Setting default reference frame to: {self.reference_frame}")

            print(f"Using the reference frame...{self.reference_frame}")
            try:
                transform = self.tf_buffer.lookup_transform(self.reference_frame.strip('/'), 'robot_footprint', rospy.Time(0), rospy.Duration(1))
                #print("====")
                #print(f"Original pose (/robot_footprint):\n {pose}")
                pose_stamped = PoseStamped()
                pose_stamped.header.frame_id = '/robot_footprint' # Assumed standard for robot
                pose_stamped.pose = pose
                pose_stamped.header.stamp = rospy.Time.now()
                pose_stamped = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform)
                pose = pose_stamped.pose
                #print(f"Transformed pose to {self.reference_frame}:\n {pose}")
                #print("====")
            except tf2_ros.LookupException as e:
                print(f"Failed to transform pose to reference frame {self.reference_frame}: {e}")
                self.reference_frame = None
                return

        print(f"Current pose: {pose}")
        # wait for user input
        print("====")
        if arg == '':
            arg = input(f"Select a property to change (pos(ition), ori(entation),link[{self.reference_frame}]): ")
        elif arg.lower() in ['orientation', 'ori', 'position', 'pos']:
            print(f"Changing {arg}...")
        else:
            print("What the hell are we doing here?")
            return

        if arg in ['', 'exit']:
            return
        elif arg.lower().startswith('/'):
            print(f"Changing pose relative to {arg}...")
            self.reference_frame = arg
            self.do_pose('')
            return

        pose_attribute_arg = arg.lower().strip()
        if pose_attribute_arg in ['position', 'pos']:
            print("Changing position...")
            print(f"Current position:\n{pose.position}")
            arg = input("Enter the position values x,y,z (separated by commas): ")
            confirm = 'n'
            if len(arg.split(',')) == 3: # A full set of pose values provided
                confirm = input(f"Move to pose: {arg} (y/n)? ")
            if confirm != 'y':
                self.do_pose('')
                return

            if len(arg.split(',')) != 3:
                print("Aborting due to incorrect number of pose values")
                return

            try:
                pose.position.x, pose.position.y, pose.position.z = [float(x) for x in arg.split(',')]
            except ValueError:
                print("Aborting due to incorrect pose value")
                return

        elif pose_attribute_arg in ['orientation', 'ori']:
            print("Changing orientation...")
            rpy = tf.transformations.euler_from_quaternion([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
            # convert to degrees
            rpy = [x * 180 / 3.14159 for x in rpy]
            print(f"Current roll, pitch, yaw (degrees):\n{rpy}")
            arg = input("Enter the roll,pitch,yaw values (separated by commas): ")
            confirm = 'n'
            if len(arg.split(',')) == 3: # A full set of pose values provided
                confirm = input(f"Move to rpy: {arg} (y/n)? ")
            if confirm != 'y':
                self.do_pose('')
                return

            if len(arg.split(',')) != 3:
                print("Aborting due to incorrect number of pose values")
                return

            try:
                rpy = [float(x) for x in arg.split(',')]
                rpy = [x * 3.14159 / 180 for x in rpy]  # convert to radians
                quat = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
                pose.orientation.x = quat[0]
                pose.orientation.y = quat[1]
                pose.orientation.z = quat[2]
                pose.orientation.w = quat[3]
            except ValueError:
                print("Aborting due to incorrect pose value")
                return
        else:
            print("Unknown property")
            return

        try:
            # Undo any previous transformations
            if self.reference_frame != '/robot_footprint':
                print(f"Transforming pose back to robot_footprint from {self.reference_frame}...")
                transform = self.tf_buffer.lookup_transform('robot_footprint', self.reference_frame.strip('/'), rospy.Time(0), rospy.Duration(1))
                pose_stamped = PoseStamped()
                pose_stamped.header.frame_id = '/robot_footprint' # Assumed standard for robot
                pose_stamped.pose = pose
                pose_stamped.header.stamp = rospy.Time.now()
                pose_stamped = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform)
                pose = pose_stamped.pose
                print(f"Transformed pose back to robot_footprint:\n {pose}")
            print(f"Moving to pose: {pose}")
            self.move_arm_to_pose(self.move_group, pose)
        except moveit_commander.MoveItCommanderException as e:
            print(f"Failed to move to pose: {pose}")
            print(e)
            print("====")
        except Exception as e:
            print(f"ERROR...Not sure what happened (dont want to loose stored locations): {e}")
            print("====")

        self.do_pose('')

    def do_joint(self, arg):
        print("JOINT based navigation...")
        self.init_move_group()

        joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        if self.sim == False:
            joint_positions = self.move_group.get_current_joint_values()

        print(f"Current joint values: {joint_positions}")
        print("====")
        confirm = 'n'
        target_joint = '0' # all joints
        if len(arg.split(',')) == 6: # A full set of joint values provided
            confirm = input(f"Move to joint configuration: {arg} (y/n)? ")
        if confirm != 'y':
            target_joint = input("Enter the joint to specify (0-all or 1-6): ")
            if target_joint == '':
                print("Aborting...")
                return False
            arg = input("Enter the joint value/s (separated by commas): ")
            if arg == '':
                print("Aborting...")
                return False

        if target_joint == '' or not target_joint.isdigit() or int(target_joint) not in range(0, 7):
            print("Aborting due to incorrect joint number")
            return
        if ((len(arg.split(',')) != 6 and target_joint == '0') or
            (len(arg.split(',')) != 1 and target_joint != '0')):
            print("Aborting due to incorrect number of joint values")
            return

        try:
            if target_joint == '0':
                joint_positions = [float(x) for x in arg.split(',')]
            else:
                joint_positions[int(target_joint)-1] = float(arg)
        except ValueError:
            print("Aborting due to incorrect joint value")
            return

        try:
            self.move_arm_to_joint_positions(self.move_group, joint_positions)
        except moveit_commander.MoveItCommanderException as e:
            print(f"Failed to move to joint position: {joint_positions}")
            print(e)
            print("====")
        except Exception as e:
            print(f"ERROR...Not sure what happened (dont want to loose stored locations): {e}")
            print("====")

        self.do_joint('')

    def do_relative(self, arg):
        print("RELATIVE based navigation...")
        self.init_move_group()

        pose = Pose()
        if self.sim == False:
            pose = self.move_group.get_current_pose().pose

            if pose is None:
                print("Current pose is None, cannot proceed with relative navigation")
                return

            # Setting default reference frame
            if self.reference_frame is None:
                self.reference_frame = '/robot_footprint' # Assumed standard for robot
                print(f"Setting default reference frame to: {self.reference_frame}")

            print(f"Using the reference frame...{self.reference_frame}")
            try:
                transform = self.tf_buffer.lookup_transform(self.reference_frame.strip('/'), 'robot_footprint', rospy.Time(0), rospy.Duration(1))
                #print("====")
                #print(f"Original pose (/robot_footprint):\n {pose}")
                pose_stamped = PoseStamped()
                pose_stamped.header.frame_id = '/robot_footprint' # Assumed standard for robot
                pose_stamped.pose = pose
                pose_stamped.header.stamp = rospy.Time.now()
                pose_stamped = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform)
                pose = pose_stamped.pose
                #print(f"Transformed pose to {self.reference_frame}:\n {pose}")
                #print("====")
            except tf2_ros.LookupException as e:
                print(f"Failed to transform pose to reference frame {self.reference_frame}: {e}")
                self.reference_frame = None
                return

        print(f"Current pose ({self.reference_frame}):\n {pose}")
        rpy = tf.transformations.euler_from_quaternion([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
        # convert to degrees
        rpy = [x * 180 / 3.14159 for x in rpy]
        print(f"Current roll, pitch, yaw (degrees): {rpy}")
        print("====")
        if arg == '':
            arg = input(f"Select a property to change (x,y,z,roll,pitch,yaw,link[{self.reference_frame}]): ")
        elif arg.lower() in ['x', 'y', 'z', 'roll', 'pitch', 'yaw']:
            print(f"Changing {arg}...")
        else:
            print("What the hell are we doing here?")
            return

        if arg in ['', 'exit']:
            return
        elif arg.lower().startswith('/'):
            print(f"Changing pose relative to {arg}...")
            self.reference_frame = arg
            self.do_relative('')
            return

        value_input = input("Enter the value <<<< WARNING Relative >>>>: ")
        if value_input == '':
            self.do_relative('')
            return
        elif value_input.replace('-','',1).replace('.','',1).isdigit():
            value = float(value_input)
        else:
            print("Incorrect value")
            return

        if arg == 'x':
            pose.position.x += value
        elif arg == 'y':
            pose.position.y += value
        elif arg == 'z':
            pose.position.z += value
        elif arg == 'roll':
            rpy[0] += value
        elif arg == 'pitch':
            rpy[1] += value
        elif arg == 'yaw':
            rpy[2] += value
        else:
            print("Unknown property")
            return

        if arg in ['roll', 'pitch', 'yaw']:
            print(f"New roll, pitch, yaw (degrees): {rpy}")
            rpy = [x * 3.14159 / 180 for x in rpy]
            quat = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
            pose.orientation.x = quat[0]
            pose.orientation.y = quat[1]
            pose.orientation.z = quat[2]
            pose.orientation.w = quat[3]

        try:
            # Undo any previous transformations
            if self.reference_frame != '/robot_footprint':
                print(f"Transforming pose back to robot_footprint from {self.reference_frame}...")
                transform = self.tf_buffer.lookup_transform('robot_footprint', self.reference_frame.strip('/'), rospy.Time(0), rospy.Duration(1))
                pose_stamped = PoseStamped()
                pose_stamped.header.frame_id = '/robot_footprint' # Assumed standard for robot
                pose_stamped.pose = pose
                pose_stamped.header.stamp = rospy.Time.now()
                pose_stamped = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform)
                pose = pose_stamped.pose
                print(f"Transformed pose back to robot_footprint:\n {pose}")
            self.move_arm_to_pose(self.move_group, pose)
        except moveit_commander.MoveItCommanderException as e:
            print(f"Failed to move to pose: {pose}")
            print(e)
            print("====")
        except Exception as e:
            print(f"ERROR...Not sure what happened (dont want to loose stored locations): {e}")
            print("====")

        self.do_relative(arg)

    def do_goto(self, arg):
        confirm = 'n'
        external_request = False
        print("GOTO location...")
        self.init_move_group()

        if arg != '':
            external_request = True # assume arg is provided externally
            if self.autonomous:
                print(f"Autonomous mode - Moving to location: {arg}")
                confirm = 'y'
            else:
                confirm = input(f"Move to location: {arg} (y/n)? ")

            if confirm != 'y':
                print("Aborting...")
                return False
        else:
            print("== home, stow, trolley-tag, calibrate-tank-X, tank-X, tag-X ==")
            arg = input("Enter the location: ")

        if arg == 'home':
            joint_positions = [-0.0160, -1.7715, 2.1785, 4.3006, -1.5685, -0.0054]
        elif arg == 'stow':
            joint_positions = [-0.0176, -2.1245, 2.4861, 4.3064, -1.5685, -0.0065]
        elif arg == 'trolley-tag':
            joint_positions = [-0.0930, -2.0006, 2.2892, 4.6090, -1.6979, 0.2121]
        elif arg == 'calibrate-tank-0':
            joint_positions = [-0.323,-1.2939, 1.7997, 4.2236, -1.5374, 1.1810]
        elif arg == 'calibrate-tank-1':
            joint_positions = [0.3344, -1.2944, 1.7997, 4.2234, -1.5375, -1.2209]
        # New 3 tag setup Tank-0
        elif arg == 'tank-0':
            joint_positions = [-1.5700, -2.1243, 2.4860, 4.3063, -1.5686, -0.0065]
        elif arg == 'tag-D':
            joint_positions = [-1.9107, -1.6746, 2.5476, 3.7970, -1.6060, 1.1929]
        elif arg == 'tag-E':
            joint_positions = [-1.1037, -0.7044, 1.3729, 3.1447, -1.1902, -0.2710]
        elif arg == 'tag-F':
            joint_positions = [-2.3286, -0.6755, 1.2908, 3.1413, -1.5731, 0.0177]
        # New 3 tag setup Tank-1
        elif arg == 'tank-1':
            joint_positions = [1.5699, -2.1243, 2.4858, 4.3062, -1.5686, -0.0066]
        elif arg == 'tag-A':
            joint_positions = [1.2805, -1.7001, 2.5585, 3.8389, -1.6246, 1.3927]
        elif arg == 'tag-B':
            joint_positions = [2.0697, -0.7345, 1.3725, 3.1419, -1.3222, -0.1646]
        elif arg == 'tag-C':
            joint_positions = [0.8162, -0.6647, 1.2615, 3.1600, -1.5964, 0.0338]
        else:
            print("Unknown location")
            return

        self.move_arm_to_joint_positions(self.move_group, joint_positions)
        if external_request == False:
            self.do_goto('')

        return True

    def cb_calibrate_done(self, status, result):
        self.calibration_done = True

    def cb_move_done(self, status, result):
        pass

    def do_tank(self, arg):
        if arg == '' or not arg.isdigit():
            return

        tank_id = int(arg)
        if tank_id not in [0, 1]:
            print("Tank ID out of range (0-1)")
            return

        print(f"Setting tank to {arg}")
        self.tank_id = tank_id
        self.do_goto(f'tank-{tank_id}')

    def do_tile(self, arg):
        print("TILE based navigation...")

        if self.sim:
            print(f"Simulating the tile move...{arg}")
            return True

        move_action_server_topic = '/cgras/robot/move'
        move_action_client = actionlib.SimpleActionClient(move_action_server_topic, MoveAction)

        # Parse the arguments
        tile_x, tile_y = 1, 1
        if arg != '':
            tile_x, tile_y = [int(x) for x in arg.split(',')]

        # Setup the goal
        the_goal = MoveGoal(type=MoveGoal.MOVE_TO_CALIBRATE_TAG, tank_id=self.tank_id, placement_x=tile_x, placement_y=tile_y,
                                 capture_x=0, capture_y=0)

        print("====")
        print(the_goal)
        print("====")

        # Send the goal
        if self.calibration_done == None:
            print("Calibrating...")
            the_goal.type = MoveGoal.MOVE_TO_CALIBRATE_TAG
            move_action_client.send_goal(the_goal, done_cb=self.cb_calibrate_done)
        elif self.calibration_done == False:
            print("-Still calibrating-")
            return
        else:
            print("Moving...")
            the_goal.type = MoveGoal.MOVE_TO_CAPTURE
            move_action_client.send_goal(the_goal, done_cb=self.cb_move_done)

    def do_exit(self, arg):
        print("Returning to main menu...")
        return True

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

        if self.sim:
            print("Simulating the move...")
            return True

        move_group.set_pose_target(pose)
        (response, plan, planning_time, error_code) = move_group.plan()
        if not response:
            rospy.logerr(f"Failed to plan to pose: {pose}")
            rospy.loginfo(f"Planning time: {planning_time}")
            rospy.loginfo(f"Error code: {error_code}")
            return False

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

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

        if self.sim:
            print("Simulating the move...")
            return True

        move_group.set_joint_value_target(joint_positions)

        # oh dear....
        # clear colliding objects
        move_group.clear_path_constraints()

        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.loginfo(f"Planning time: {planning_time}")
            rospy.loginfo(f"Error code: {error_code}")
            return False

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

    def init_move_group(self):
        if self.move_group is None and self.sim == False:
            self.move_group = moveit_commander.MoveGroupCommander("manipulator")
            self.tf_buffer = tf2_ros.Buffer()
            self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

    def set_sim(self, value):
        print(f"Setting NAV simulation to...{value}")
        self.sim = value

    def set_autonomous(self, value):
        self.autonomous = value

    def __init__(self):
        super().__init__()
        self.move_group = None
        self.tf_buffer = None
        self.tf_listener = None
        self.reference_frame = None
        self.calibration_done = None
        self.captured_joint_values = []
        self.autonomous = False
        self.sim = False
        self.tank_id = 1

    def __del__(self):
        print("Shutting down NavigateTools...")
        self.move_group = None

class ScriptedSequence(cmd.Cmd):
    prompt = "Scripted: run or capture > "

    def emptyline(self):
        print("Exiting...")
        return True

    def do_exit(self, arg):
        print("Returning to main menu...")
        return True

    def do_capture(self, arg):
        print("CAPTURING joint values...")
        print("====")

        # TODO: Risky dependance on script name...
        if 'trolley' in arg:
            print("Trolley calibration initiated")
            self.set_camera_calibration(True)
            self.init_handeye_calibration()
        else:
            self.set_camera_calibration(False)

        self.capture_fn = self.capture_joints
        run_success = self.do_run(arg)
        self.capture_fn = None

        if run_success == False:
            print("Run failed, Aborting the capture...")
            return False

        # Write
        print("====")
        print("Capturing complete")

        if self.camera_calibration == True:
            print("Camera calibration is ON, running hand-eye calibration...")
            self.run_handeye_calibration()
            self.camera_calibration = False

        if arg != '':
            current_time = time.strftime("%Y%m%d-%H%M%S")
            file_name = arg.split('.')[0] + f'-sample-captured-{current_time}.yaml'
        elif self.autonomous:
            print("Autonomous mode without filename...weird...pretend we are done")
            self.capture_results = None
            return True
        else:
            file_name = input("Enter the file name to save: ")

        if file_name == '':
            print("Aborting due to empty file name")
            self.capture_results = None
            return False

        print(f"Saving the captured data to {file_name}")
        with open(f'./calibration/{file_name}', 'w') as the_file:
            yaml.dump(self.capture_results, the_file)

        self.capture_results = None

        # IF this is a manual capture, then we can continue capturing
        if arg == '':
            self.do_capture('')

        return True

    # TODO: Different file types...
    def do_run(self, arg):
        # TODO: Autocomplete file names in the calibration folder
        confirm = 'n'
        if arg != '':
            if self.autonomous:
                confirm = 'y'
            else:
                confirm = input(f"Rerun\n {arg}\n (y/n)? ")

            if confirm != 'y':
                print("Aborting...")
                return False
        else:
            arg = input("Enter the file path: ")

        if arg == '':
            print("Aborting due to empty file path")
            return
        if not os.path.exists(f'./calibration/{arg}'):
            print("File does not exist")
            return

        # Check if we stop between each command
        if self.autonomous:
            stop_between = 'n'
        else:
            stop_between = input("Stop between each command (y/n)? ")

        # Read
        with open(f'./calibration/{arg}', newline='') as the_file:
            joint_state_dict = yaml.safe_load(the_file)
            joint_values = joint_state_dict['joint_values']
            print(f"Number of locations read: {len(joint_values)}")

        # Move
        self.init_move_group()
        for idx, joint_position in enumerate(joint_values):
            print(f"Moving to joint position {idx+1}/{len(joint_values)}: {joint_position}\n")

            try:
                self.move_arm_to_joint_positions(self.move_group, joint_position)
            except moveit_commander.MoveItCommanderException as e: 
                print(f"Failed to move to joint position {idx+1}/{len(joint_values)}: {joint_position}")
                print(e)

            time.sleep(1)
            if self.capture_fn is not None:
                self.capture_fn()
            if stop_between == 'y':
                input("Press Enter to continue...")

        if self.capture_fn is not None:
            return

        self.do_run(arg)

    def capture_joints(self):
        if self.capture_results is None:
            self.capture_results = []

        if self.sim:
            print("Simulating the capture...")
            return True

        print("Setting up the listener...")
        self.init_transform_listener()
        try:
            object_pose = self.get_transform_as_pose(self.tf_buffer, 'camera_color_optical_frame', 'handeye_target')
            effector_pose = self.get_transform_as_pose(self.tf_buffer, 'base_link', 'tool0')
        except tf.ExtrapolationException as e:
            rospy.logerr(f'Extrapolation exception received: {e}')
            return

        if object_pose is None or effector_pose is None:
            return

        T_obj_xyz = np.array([object_pose.position.x, object_pose.position.y, object_pose.position.z])
        obj_rpy = tf.transformations.euler_from_quaternion([object_pose.orientation.x, object_pose.orientation.y, object_pose.orientation.z, object_pose.orientation.w])
        T_obj_rpy = np.array(obj_rpy)
        T_obj = np.eye(4)
        T_obj[:3, 3] = T_obj_xyz
        r = R.from_euler('xyz', T_obj_rpy)
        T_obj[:3, :3] = r.as_matrix()

        T_eff_xyz = np.array([effector_pose.position.x, effector_pose.position.y, effector_pose.position.z])
        eff_rpy = tf.transformations.euler_from_quaternion([effector_pose.orientation.x, effector_pose.orientation.y, effector_pose.orientation.z, effector_pose.orientation.w])
        T_eff_rpy = np.array(eff_rpy)
        T_eff = np.eye(4)
        T_eff[:3, 3] = T_eff_xyz
        r = R.from_euler('xyz', T_eff_rpy)
        T_eff[:3, :3] = r.as_matrix()

        # list of rows of T_obj
        object_pose_list = T_obj.flatten().tolist()
        effector_pose_list = T_eff.flatten().tolist()
        self.capture_results.append({'effector_wrt_world': effector_pose_list, 'object_wrt_sensor': object_pose_list})

        if self.camera_calibration == True:
            self.calibration_samples.append({'effector_wrt_world': effector_pose, 'object_wrt_sensor': object_pose})

    def get_transform_as_pose(self, tf_buffer, from_frame, to_frame):
        try:
            transform = tf_buffer.lookup_transform(from_frame, to_frame, rospy.Time(0), rospy.Duration(1))
            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 tf2_ros.LookupException as e:
            rospy.logerr(f'Lookup exception received: {e}')
            return None
        except tf.ExtrapolationException as e:
            rospy.logerr(f'Extrapolation exception received: {e}')
            return None
        return pose

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

        if self.sim:
            print("Simulating the move...")
            return True

        move_group.set_pose_target(pose)
        (response, plan, planning_time, error_code) = move_group.plan()
        if not response:
            rospy.logerr(f"Failed to plan to pose: {pose}")
            rospy.loginfo(f"Planning time: {planning_time}")
            rospy.loginfo(f"Error code: {error_code}")
            return False

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

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

        if self.sim:
            print("Simulating the move...")
            return True

        move_group.set_joint_value_target(joint_positions)

        # oh dear....
        # clear colliding objects
        move_group.clear_path_constraints()

        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.loginfo(f"Planning time: {planning_time}")
            rospy.loginfo(f"Error code: {error_code}")
            return False

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

    def init_move_group(self):
        if self.move_group is None and self.sim == False:
            self.move_group = moveit_commander.MoveGroupCommander("manipulator")

    def init_transform_listener(self):
        print("Setting up (inside) the listener...")
        if self.tf_buffer is None and self.sim == False:
            print("The listener is not set up")
            self.tf_buffer = tf2_ros.Buffer()
            self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
            print("The listener is NOW set up")

    def init_handeye_calibration(self):
        if self.move_group is None and self.sim == False:
            self.move_group = moveit_commander.MoveGroupCommander("manipulator")

        if self.tf_buffer is None and self.sim == False:
            print("Setting up the transform listener...")
            self.tf_buffer = tf2_ros.Buffer()
            self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        if self.tf_publisher is None:
            self.tf_publisher = tf.TransformBroadcaster()

        srv_topic = '/handeye_calibration'
        print(f"Waiting for service {srv_topic}...")
        rospy.wait_for_service(srv_topic)
        print(f"Service {srv_topic} is available, setting up the service proxy...")
        self.calibrate_srv = rospy.ServiceProxy(srv_topic, CalibrateHandEye)

        self.calibration_request = CalibrateHandEyeRequest()
        self.calibration_request.setup = 'Moving'
        #solver = 'ParkBryan1994'
        #solver = 'TsaiLenz1989'
        solver = 'Daniilidis1999'
        self.calibration_request.solver = solver
        self.calibration_samples = []

    def run_handeye_calibration(self):

        #request.effector_wrt_world.poses.append(move_group.get_current_pose().pose)
        # get transform from camera to handeye_target

        if self.calibration_samples is None or len(self.calibration_samples) == 0:
            rospy.logerr("No capture results available for calibration")
            return False

        for i in range(len(self.calibration_samples)):
            object_pose = None
            effector_pose = None

            try:
                object_pose = self.calibration_samples[i]['object_wrt_sensor']
                effector_pose = self.calibration_samples[i]['effector_wrt_world']
            except KeyError as e:
                rospy.logerr(f"Key error in capture results: {e}")
                return False

            if object_pose is None or effector_pose is None:
                return False

            if self.calibration_request is None or self.calibration_request.effector_wrt_world.poses is None or self.calibration_request.object_wrt_sensor.poses is None:
                print("Hand-eye calibration request is not initialized")
                return False

            self.calibration_request.effector_wrt_world.poses.append(effector_pose)
            self.calibration_request.object_wrt_sensor.poses.append(object_pose)

        if self.calibrate_srv is None:
            print("Hand-eye calibration service is not initialized")
            return False
        try:
            response = self.calibrate_srv(self.calibration_request)
            print(f"Calibration service using {self.calibration_request.solver} returned: {response}")

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

            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")

            # get the transform from the camera_link to camera_color_optical_frame and subtract it from the effector to sensor transform
            try:
                camera_to_optical = self.tf_buffer.lookup_transform('camera_link', 'camera_color_optical_frame', rospy.Time(0), rospy.Duration(1))
                print(f"Camera to optical frame transform: {camera_to_optical}")
                eff_to_sensor.position.x -= camera_to_optical.transform.translation.x
                eff_to_sensor.position.y -= camera_to_optical.transform.translation.y
                eff_to_sensor.position.z -= camera_to_optical.transform.translation.z

                sensor_orientation = R.from_quat([camera_to_optical.transform.rotation.x, camera_to_optical.transform.rotation.y,
                                                  camera_to_optical.transform.rotation.z, camera_to_optical.transform.rotation.w])
                eff_orientation = R.from_quat([eff_to_sensor.orientation.x, eff_to_sensor.orientation.y,
                                                eff_to_sensor.orientation.z, eff_to_sensor.orientation.w])
                eff_orientation = eff_orientation * sensor_orientation.inv()
                eff_to_sensor.orientation.x = eff_orientation.as_quat()[0]
                eff_to_sensor.orientation.y = eff_orientation.as_quat()[1]
                eff_to_sensor.orientation.z = eff_orientation.as_quat()[2]
                eff_to_sensor.orientation.w = eff_orientation.as_quat()[3]
            except tf2_ros.LookupException as e:
                rospy.logerr(f'Lookup exception received: {e}')
                return False

            # Save the transform for later use...
            self.camera_transform = eff_to_sensor

            print(f"Adjusted effector to sensor transform: {eff_to_sensor}")
            input("Press Enter to continue...")

            # publish transform until the user presses a key
            print("Publishing the transform from effector to sensor...")
            print("Press Ctrl+C to stop publishing the transform")
            from inputimeout import inputimeout, TimeoutOccurred
            while not rospy.is_shutdown():
                rospy.sleep(0.1)
                try:
                    inputimeout(prompt="Press Enter to stop publishing the transform...", timeout=1.0)
                    print("Stopping the transform publishing")
                    break
                except TimeoutOccurred:
                    pass

                if self.tf_publisher is None:
                    print("Transform publisher is not initialized")
                    return False
                if eff_to_sensor is None:
                    print("Effector to sensor transform is not available")
                    return False
                # publish transform from effector to sensor
                self.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_link",
                                 "tool0")
            #self.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}")
            return False
        return True

    def set_sim(self, value):
        print(f"Setting SCRIPT simulation...{value}")
        self.sim = value

    def set_autonomous(self, value):
        self.autonomous = value

    def set_camera_calibration(self, value):
        print(f"Setting camera calibration...{value}")
        self.camera_calibration = value

    def __init__(self):
        super().__init__()
        print("Initializing the class...")
        self.move_group = None
        self.capture_results = None
        self.capture_fn = None
        self.tf_buffer = None
        self.tf_publisher = None
        self.sim = False
        self.autonomous = False
        self.calibrate_srv = None
        self.calibration_request = None
        self.calibration_samples = []
        self.camera_transform = None

    def __del__(self):
        print("Shutting down ScriptedSequence...")
        self.move_group = None

class CalibrationMenu(cmd.Cmd):
    prompt = "Calibrate: trolley or tank > "

    def emptyline(self):
        print("Exiting...")
        if self.script is not None and self.script.camera_transform is not None:
            print("Saving the camera transform...")
            #with open('./calibration/camera_transform.yaml', 'w') as file:
            #    yaml.dump(self.script.camera_transform, file)
            self.camera_transform = self.script.camera_transform
        return True

    def do_autonomous(self, arg):
        if arg == 'on':
            print("Setting autonomous mode ON")
            self.set_autonomous(True)
        elif arg == 'off':
            print("Setting autonomous mode OFF")
            self.set_autonomous(False)
        else:
            print("Invalid argument, use 'on' or 'off'")

    def do_tank(self, arg):
        ## New 3 Tag Setup - Tank 1
        tag_A_script = 'tank-1-tag-A.yaml'
        tag_B_script = 'tank-1-tag-B.yaml'
        tag_C_script = 'tank-1-tag-C.yaml'
        ## New 3 Tag Setup - Tank 0
        tag_D_script = 'tank-0-tag-D.yaml'
        tag_E_script = 'tank-0-tag-E.yaml'
        tag_F_script = 'tank-0-tag-F.yaml'

        tank_0 = ('tank-0', ('tag-D', tag_D_script), ('tag-E', tag_E_script), ('tag-F', tag_F_script))
        tank_1 = ('tank-1', ('tag-A', tag_A_script), ('tag-B', tag_B_script), ('tag-C', tag_C_script))

        tank_list = []

        if arg == '':
            tank_list = [tank_0, tank_1]
        elif arg == '0':
            tank_list = [tank_0]
        elif arg == '1':
            tank_list = [tank_1]

        target_dim_srv_topic = '/handeye_target_charuco_detector/set_target_dimension'
        rospy.wait_for_service(target_dim_srv_topic)
        target_dim_srv = rospy.ServiceProxy(target_dim_srv_topic, TargetDimension)
        longest_board_size = 0.1970
        measured_marker_size = 0.0170
        response = target_dim_srv(longest_board_size, measured_marker_size)
        print(f"Target dimensions set: {response}")

        for tank in tank_list:
            tank_entry_string, entry_tag, left_tag, right_tag = tank

            print(f"Calibrating tank - {tank_entry_string}...")
            print("====")

            print("- Starting from the stow position")
            if self.nav.do_goto('stow') == False:
                return False
            print("====")
            print(f"- Moving to {tank_entry_string} starting position")
            if self.nav.do_goto(tank_entry_string) == False:
                return False
            print("====")
            entry_tag_name = entry_tag[0]
            entry_tag_script = entry_tag[1]
            print(f"- Moving to the entry calibration tag - {entry_tag_name}")
            if self.nav.do_goto(entry_tag_name) == False:
                return False
            print("====")
            print("- Capturing with - " + entry_tag_script)
            if self.script.do_capture(entry_tag_script) == False:
                return False
            print("====")
            print(f"- Moving to {tank_entry_string} starting position")
            if self.nav.do_goto(tank_entry_string) == False:
                return False
            print("====")
            left_tag_name = left_tag[0]
            left_tag_script = left_tag[1]
            print(f"- Moving to the left calibration tag - {left_tag_name}")
            if self.nav.do_goto(left_tag_name) == False:
                return False
            print("====")
            print("- Capturing with - " + left_tag_script)
            if self.script.do_capture(left_tag_script) == False:
                return False
            print("====")
            print(f"- Moving to {tank_entry_string} starting position")
            if self.nav.do_goto(tank_entry_string) == False:
                return False
            print("====")
            right_tag_name = right_tag[0]
            right_tag_script = right_tag[1]
            print(f"- Moving to the right calibration tag - {right_tag_name}")
            if self.nav.do_goto(right_tag_name) == False:
                return False
            print("====")
            print("- Capturing with - " + right_tag_script)
            if self.script.do_capture(right_tag_script) == False:
                return False
            print("====")
            print(f"- Moving to {tank_entry_string} starting position")
            if self.nav.do_goto(tank_entry_string) == False:
                return False
            print("====")

        print("- <<< Calibration complete >>>")
        print("====")
        print("- Returning to the stow position")
        if self.nav.do_goto('stow') == False:
            return False
        print("====")
        return True

    # NOTE <<< Untested >>>
    def do_trolley(self, arg):
        print("Calibrating the trolley...")
        print("====")
        target_dim_srv_topic = '/handeye_target_charuco_detector/set_target_dimension'
        rospy.wait_for_service(target_dim_srv_topic)
        target_dim_srv = rospy.ServiceProxy(target_dim_srv_topic, TargetDimension)
        longest_board_size = 0.394
        measured_marker_size = 0.035
        response = target_dim_srv(longest_board_size, measured_marker_size)

        trolley_tag_name = 'trolley-tag'
        trolley_tag_script = 'trolley-tag.yaml'

        print(f"Target dimensions set: {response}")

        print("- Starting from the stow position")
        if self.nav.do_goto('stow') == False:
            return False
        print("====")
        print("- Moving to the calibration tag")
        if self.nav.do_goto(trolley_tag_name) == False:
            return False
        print("====")
        print("- Capturing the joint values")
        # TODO: Validate this joint state file
        if self.script.do_capture(trolley_tag_script) == False:
            return False

        if self.script.camera_transform is not None:
            self.camera_transform = self.script.camera_transform
            print(f"- Saving Camera Transform {self.camera_transform}")

        print("====")
        print("- <<< Calibration complete >>>")
        print("====")
        print("- Returning to the stow position")
        if self.nav.do_goto('stow') == False:
            return False
        print("====")
        return True

    def set_sim(self, value):
        print(f"Setting CALIBRATION simulation...{value}")
        self.sim = value
        self.nav.set_sim(value)
        self.script.set_sim(value)

    def set_autonomous(self, value):
        print(f"Setting CALIBRATION autonomous...{value}")
        self.autonomous = value
        self.nav.set_autonomous(value)
        self.script.set_autonomous(value)

    def __init__(self):
        super().__init__()
        self.nav = NavigateTools()
        self.script = ScriptedSequence()
        self.autonomous = False
        self.sim = False
        self.camera_transform = None

class DemoMenu(cmd.Cmd):
    prompt = "Demo: scan, calibrate_and_scan, tank_calibration, trolley_calibration > "

    def emptyline(self):
        print("Exiting...")
        return True

    def do_scan(self, arg):
        self.cal.set_autonomous(True)
        self.nav.set_autonomous(True)

        print("Demo SCAN...")
        print("====")
        # Top left two tiles
        self.nav.do_tile('1,0')
        self.nav.do_tile('2,0')
        # Top right two tiles
        self.nav.do_tile('6,0')
        self.nav.do_tile('7,0')
        # Middle left and right tile
        self.nav.do_tile('0,1')
        self.nav.do_tile('8,1')
        # Bottom left and right tile
        self.nav.do_tile('0,2')
        self.nav.do_tile('8,2')
        print("====")
        print("Returning to the stow position")
        self.nav.do_goto('stow')

        self.cal.set_autonomous(False)
        self.nav.set_autonomous(False)

        count = 0
        if arg != '' and arg.isdigit():
            count = int(arg) -1
            count = count if count > 0 else 0

        if count > 0:
            print(f"Repeating the another {count} times")
            return self.do_scan(str(count))
        else:
            print("Demo complete")
            print("====")
            return False

    def do_trolley_calibration(self, arg):
        self.cal.set_autonomous(True)
        self.nav.set_autonomous(True)

        print("Demo TROLLEY calibration...")
        print("====")
        self.cal.do_trolley('')
        print("====")
        print("Returning to the stow position")
        self.nav.do_goto('stow')
        print("====")

        self.cal.set_autonomous(False)
        self.nav.set_autonomous(False)

        count = 0
        if arg != '' and arg.isdigit():
            count = int(arg) -1
            count = count if count > 0 else 0

        if count > 0:
            print(f"Repeating the another {count} times")
            return self.do_trolley_calibration(str(count))
        else:
            print("Demo complete")
            print("====")
            return False

    def do_tank_calibration(self, arg):
        self.cal.set_autonomous(True)
        self.nav.set_autonomous(True)

        print("Demo TANK calibration...")
        print("====")
        self.cal.do_tank('')
        print("====")
        print("Returning to the stow position")
        self.nav.do_goto('stow')
        print("====")

        self.cal.set_autonomous(False)
        self.nav.set_autonomous(False)

        count = 0
        if arg != '' and arg.isdigit():
            count = int(arg) -1
            count = count if count > 0 else 0

        if count > 0:
            print(f"Repeating the another {count} times")
            return self.do_tank_calibration(str(count))
        else:
            print("Demo complete")
            print("====")
            return False

    def do_calibrate_and_scan(self, arg):
        self.cal.set_autonomous(True)
        self.nav.set_autonomous(True)

        print("Demo calibrate & scan...")
        print("====")
        self.cal.do_trolley('')
        print("====")
        self.cal.do_tank('')
        print("====")
        ## Top left two tiles
        #self.nav.do_tile('1,0')
        #self.nav.do_tile('2,0')
        ## Top right two tiles
        #self.nav.do_tile('6,0')
        #self.nav.do_tile('7,0')
        ## Middle left and right tile
        #self.nav.do_tile('0,1')
        #self.nav.do_tile('8,1')
        ## Bottom left and right tile
        #self.nav.do_tile('0,2')
        #self.nav.do_tile('8,2')
        print("====")
        print("Returning to the stow position")
        self.nav.do_goto('stow')
        print("====")
        
        self.cal.set_autonomous(False)
        self.nav.set_autonomous(False)

        count = 0
        if arg != '' and arg.isdigit():
            count = int(arg) -1
            count = count if count > 0 else 0

        if count > 0:
            print(f"Repeating the another {count} times")
            return self.do_calibrate_and_scan(str(count))
        else:
            print("Demo complete")
            print("====")
            return False

    def set_sim(self, value):
        print(f"Setting DEMO simulation to ...{value}")
        self.sim = value
        self.cal.set_sim(value)
        self.nav.set_sim(value)

    def set_autonomous(self, value):
        print(f"Setting DEMO autonomous to ...{value}")
        self.autonomous = value
        self.cal.set_autonomous(value)
        self.nav.set_autonomous(value)

    def __init__(self):
        super().__init__()
        self.cal = CalibrationMenu()
        self.nav = NavigateTools()
        self.sim = False
        self.autonomous = False

class MainMenu(cmd.Cmd):
    prompt = "demo, cal-ibrate, scr-ipt, nav-igate, or exit > "

    def emptyline(self):
        return False

    def do_sim(self, arg):
        print(f"Setting simulation to {self.sim == False}")
        self.sim = not self.sim

    def do_tank(self, arg):
        print("Setting Target Detection to TANK")
        target_dim_srv_topic = '/handeye_target_charuco_detector/set_target_dimension'
        rospy.wait_for_service(target_dim_srv_topic)
        target_dim_srv = rospy.ServiceProxy(target_dim_srv_topic, TargetDimension)
        longest_board_size = 0.1970
        measured_marker_size = 0.0170
        response = target_dim_srv(longest_board_size, measured_marker_size)
        print(f"Target dimensions set: {response}")

    def do_trolley(self, arg):
        print("Setting Target Detection to TROLLEY")
        target_dim_srv_topic = '/handeye_target_charuco_detector/set_target_dimension'
        rospy.wait_for_service(target_dim_srv_topic)
        target_dim_srv = rospy.ServiceProxy(target_dim_srv_topic, TargetDimension)
        longest_board_size = 0.394
        measured_marker_size = 0.035
        response = target_dim_srv(longest_board_size, measured_marker_size)
        print(f"Target dimensions set: {response}")

    def do_transform(self, arg):
        print("Publishing the transform from effector to sensor...")
        print("Press Ctrl+C to stop publishing the transform")
        from inputimeout import inputimeout, TimeoutOccurred
        tf_publisher = tf.TransformBroadcaster()
        while not rospy.is_shutdown():
            rospy.sleep(0.1)
            try:
                inputimeout(prompt="Press Enter to stop publishing the transform...", timeout=1.0)
                print("Stopping the transform publishing")
                break
            except TimeoutOccurred:
                pass

            if tf_publisher is None:
                print("Transform publisher is not initialized")
                return False

            eff_to_sensor = Pose()
            if self.camera_transform is None:
                print("Effector to sensor transform is not available...using default")
                eff_to_sensor.position.x = 0.03735035741427168
                eff_to_sensor.position.y = -0.03592544022421186
                eff_to_sensor.position.z = 0.1248918793924123
                eff_to_sensor.orientation.x = -0.5033185701643866
                eff_to_sensor.orientation.y = -0.4972823381935274
                eff_to_sensor.orientation.z = -0.5048539744998409
                eff_to_sensor.orientation.w = 0.4944726053889839
            else:
                eff_to_sensor = self.camera_transform
            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_link",
                             "tool0")

    def do_demo(self, arg):
        print("Demo...")
        demo = DemoMenu()
        demo.set_sim(self.sim)
        demo.cmdloop()

    def do_cal(self, arg):
        print("Calibrating...")
        cal = CalibrationMenu()
        cal.set_sim(self.sim)
        cal.cmdloop()
        if cal.camera_transform is not None:
            self.camera_transform = cal.camera_transform

    def do_scr(self, arg):
        print("launching a SCRIPT...", arg)
        scr = ScriptedSequence()
        scr.set_sim(self.sim)
        scr.cmdloop()

    def do_nav(self, arg):
        print("Navigating...")
        nav = NavigateTools()
        nav.set_sim(self.sim)
        nav.cmdloop()

    def do_exit(self, arg):
        print("Exiting...")
        return True

    def __init__(self):
        super().__init__()
        rospy.init_node('calibration_navigation_node')
        moveit_commander.roscpp_initialize(sys.argv)
        self.sim = False
        self.camera_transform = None

    def __del__(self):
        print("Shutting down MoveIt...")
        # Shutdown MoveIt
        moveit_commander.roscpp_shutdown()
        rospy.signal_shutdown("Exiting...")

if __name__ == '__main__':
    MainMenu().cmdloop()