def __init__(self):
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'failed', 'no_object_for_ppt_platform'],
            input_keys=[
                'base_pose_to_approach', 'last_grasped_obj', 'move_arm_to',
                'move_base_by', 'object_pose', 'rear_platform_free_poses',
                'rear_platform_occupied_poses', 'selected_objects',
                'next_arm_pose_index', 'cavity_pose', 'task_list'
            ],
            output_keys=[
                'base_pose_to_approach', 'last_grasped_obj', 'move_arm_to',
                'move_base_by', 'rear_platform_free_poses',
                'rear_platform_occupied_poses', 'selected_objects',
                'next_arm_pose_index', 'cavity_pose', 'task_list'
            ])

        with self:

            # TODO: ENSURE WE HAVE ENOUGH SPACE IN THE ARENA
            smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE',
                                   gns.adjust_to_workspace(0.2),
                                   transitions={
                                       'succeeded':
                                       'SELECT_OBJECTS_TO_PLACE',
                                       'failed':
                                       'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'
                                   })

            # Select object from task list which needs to be place in cavities.
            smach.StateMachine.add('SELECT_OBJECTS_TO_PLACE',
                                   ppts.select_objects_to_place(),
                                   transitions={
                                       'objects_selected':
                                       'CLEAR_CAVITIES',
                                       'no_more_obj_for_this_workspace':
                                       'no_object_for_ppt_platform'
                                   })

            smach.StateMachine.add(
                'CLEAR_CAVITIES',
                ppts.clear_cavities(),
                transitions={'succeeded': 'SELECT_NEXT_LOOK_POSE'})

            # CAVITY RECOGNITION PIPELINE
            smach.StateMachine.add('SELECT_NEXT_LOOK_POSE',
                                   gms.select_arm_pose([
                                       'look_at_workspace_right',
                                       'look_at_workspace',
                                       'look_at_workspace_left'
                                   ]),
                                   transitions={
                                       'succeeded': 'LOOK_AROUND',
                                       'failed': 'CHECK_FOUND_CAVITIES'
                                   })

            smach.StateMachine.add('LOOK_AROUND',
                                   gms.move_arm(),
                                   transitions={
                                       'succeeded': 'FIND_CAVITIES_LOOP',
                                       'failed': 'LOOK_AROUND'
                                   })

            smach.StateMachine.add('FIND_CAVITIES_LOOP',
                                   gbs.loop_for(3),
                                   transitions={
                                       'loop': 'FIND_CAVITIES',
                                       'continue': 'CHECK_FOUND_CAVITIES'
                                   })

            smach.StateMachine.add('FIND_CAVITIES',
                                   gps.find_cavities(),
                                   transitions={
                                       'succeeded': 'TRANSFORM_CAVITIES',
                                       'failed': 'SELECT_NEXT_LOOK_POSE'
                                   })

            smach.StateMachine.add(
                'TRANSFORM_CAVITIES',
                gps.transform_object_poses(frame_id='/odom'),
                transitions={'succeeded': 'FIND_BEST_MATCHED_CAVITIES'},
                remapping={'found_objects': 'found_cavities'})

            smach.StateMachine.add('FIND_BEST_MATCHED_CAVITIES',
                                   gps.find_best_matched_cavities(),
                                   transitions={
                                       'succeeded': 'SELECT_NEXT_LOOK_POSE',
                                       'complete': 'CHECK_FOUND_CAVITIES'
                                   })

            smach.StateMachine.add('CHECK_FOUND_CAVITIES',
                                   gps.check_found_cavities(),
                                   transitions={
                                       'cavities_found':
                                       'STOP_ALL_COMPONENTS_AT_START',
                                       'no_cavities_found': 'failed'
                                   })

            smach.StateMachine.add(
                'STOP_ALL_COMPONENTS_AT_START',
                gbs.send_event([
                    ('/gripper_to_object_pose_error_calculator/event_in',
                     'e_stop'),
                    ('/mcr_common/relative_displacement_calculator/event_in',
                     'e_stop'), ('/pregrasp_planner/event_in', 'e_stop'),
                    ('/pregrasp_arm_monitor/event_in', 'e_stop'),
                    ('/mcr_navigation/relative_base_controller/event_in',
                     'e_stop'), ('/planned_motion/event_in', 'e_stop')
                ]),
                transitions={'success': 'SELECT_OBJECT_TO_PLACE'})

            # OBJECT PLACING pipeline
            # select cavitiy based on recognized cavities and list of objects to place in cavity.
            smach.StateMachine.add(
                'SELECT_OBJECT_TO_PLACE',
                ppts.select_object_to_place(),
                transitions={
                    'object_selected': 'COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
                    'no_more_objects': 'succeeded',
                    'no_more_cavities': 'succeeded'
                },
                remapping={'found_objects': 'best_matched_cavities'})

            # ARM AND BASE ALIGNMENT TO CAVITY PIPELINE
            smach.StateMachine.add(
                'COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
                gbs.send_event([
                    ('/pregrasp_planner/event_in', 'e_start'),
                    ('/gripper_to_object_pose_error_calculator/event_in',
                     'e_start'),
                    ('/mcr_common/relative_displacement_calculator/event_in',
                     'e_start')
                ]),
                transitions={
                    'success': 'WAIT_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY'
                })

            smach.StateMachine.add(
                'WAIT_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
                gbs.wait_for_events([
                    ('/pregrasp_planner/event_out', 'e_success', True),
                    ('/gripper_to_object_pose_error_calculator/event_out',
                     'e_success', True),
                    ('/mcr_common/relative_displacement_calculator/event_out',
                     'e_done', True)
                ]),
                transitions={
                    'success': 'STOP_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
                    'timeout':
                    'STOP_ALL_COMPONENTS',  #should we try again for compute arm base shift
                    'failure': 'STOP_ALL_COMPONENTS'
                })  #should we try again for compute arm base shift

            smach.StateMachine.add(
                'STOP_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
                gbs.send_event([
                    ('/gripper_to_object_pose_error_calculator/event_in',
                     'e_stop'), ('/pregrasp_planner/event_in', 'e_stop'),
                    ('/mcr_common/relative_displacement_calculator/event_in',
                     'e_stop')
                ]),
                transitions={'success': 'ALIGN_BASE_WITH_CAVITY'})

            smach.StateMachine.add(
                'ALIGN_BASE_WITH_CAVITY',
                gbs.send_event([
                    ('/mcr_navigation/relative_base_controller/event_in',
                     'e_start')
                ]),
                transitions={'success': 'WAIT_ALIGN_BASE_WITH_CAVITY'})

            smach.StateMachine.add(
                'WAIT_ALIGN_BASE_WITH_CAVITY',
                gbs.wait_for_events([
                    ('/mcr_navigation/relative_base_controller/event_out',
                     'e_done', True),
                    ('/mcr_navigation/collision_velocity_filter/event_out',
                     'e_zero_velocities_forwarded', False)
                ]),
                transitions={
                    'success': 'STOP_ALIGN_BASE_WITH_CAVITY',
                    'timeout':
                    'STOP_ALL_COMPONENTS',  #should we try again for compute and aligh arm and base to object 
                    'failure': 'STOP_ALL_COMPONENTS'
                }
            )  #should we try again for compute and aligh arm and base to object

            smach.StateMachine.add(
                'STOP_ALIGN_BASE_WITH_CAVITY',
                gbs.send_event([
                    ('/mcr_navigation/relative_base_controller/event_in',
                     'e_stop')
                ]),
                transitions={'success': 'GRASP_OBJECT_FOR_CAVITY_FROM_PLTF'})

            # make sure that gripper is open
            smach.StateMachine.add('GRASP_OBJECT_FOR_CAVITY_FROM_PLTF',
                                   ppts.grasp_obj_for_hole_from_pltf(),
                                   transitions={
                                       'object_grasped':
                                       'MOVE_ARM_TO_CAVITY',
                                       'no_more_obj_for_this_workspace':
                                       'no_object_for_ppt_platform'
                                   })

            smach.StateMachine.add('GO_TO_PREGRASP',
                                   gms.move_arm('pre_grasp'),
                                   transitions={
                                       'succeeded': 'MOVE_ARM_TO_CAVITY',
                                       'failed': 'MOVE_ARM_TO_CAVITY'
                                   })

            smach.StateMachine.add(
                'MOVE_ARM_TO_CAVITY',
                gbs.send_event([
                    ('/planned_motion/event_in', 'e_start'),
                    #smach.StateMachine.add('MOVE_ARM_TO_CAVITY', gbs.send_event([('/planned_motion/event_in', 'e_start')]),
                    ('/pregrasp_arm_monitor/event_in', 'e_start')
                ]),
                transitions={'success': 'WAIT_MOVE_ARM_TO_CAVITY'})

            smach.StateMachine.add(
                'WAIT_MOVE_ARM_TO_CAVITY',
                gbs.wait_for_events([('/pregrasp_arm_monitor/event_out',
                                      'e_done', True)]),
                #          smach.StateMachine.add('WAIT_MOVE_ARM_TO_CAVITY', gbs.wait_for_events([('/planned_motion/event_out','e_success', True)]),
                #                                                                             ('/pregrasp_arm_monitor/event_out', 'e_done', True)]),
                transitions={
                    'success': 'STOP_MOVE_ARM_TO_CAVITY',
                    'timeout':
                    'STOP_ALL_COMPONENTS',  #should we try again for compute and aligh arm and base to object 
                    'failure': 'STOP_ALL_COMPONENTS'
                }
            )  #should we try again for compute and aligh arm and base to object

            #smach.StateMachine.add('STOP_MOVE_ARM_TO_CAVITY', gbs.send_event([('/planned_motion/event_in','e_stop')]),
            smach.StateMachine.add(
                'STOP_MOVE_ARM_TO_CAVITY',
                gbs.send_event([('/planned_motion/event_in', 'e_stop'),
                                ('/pregrasp_arm_monitor/event_in', 'e_stop')]),
                transitions={'success': 'MOVE_ARM_TO_INTERMEDIATE_POSE'})

            smach.StateMachine.add('PLACE_OBJECT',
                                   gms.linear_motion(operation='release'),
                                   transitions={
                                       'succeeded': 'WIGGLE_ARM_LEFT',
                                       'failed': 'PLACE_OBJECT'
                                   })

            smach.StateMachine.add('WIGGLE_ARM_LEFT',
                                   ppts.ppt_wiggle_arm(wiggle_offset=-0.12),
                                   transitions={
                                       'succeeded': 'WIGGLE_ARM_RIGHT',
                                       'failed': 'WIGGLE_ARM_RIGHT'
                                   })

            smach.StateMachine.add('WIGGLE_ARM_RIGHT',
                                   ppts.ppt_wiggle_arm(wiggle_offset=0.24),
                                   transitions={
                                       'succeeded':
                                       'MOVE_ARM_TO_INTERMEDIATE_POSE',
                                       'failed':
                                       'MOVE_ARM_TO_INTERMEDIATE_POSE'
                                   })

            smach.StateMachine.add('MOVE_ARM_TO_INTERMEDIATE_POSE',
                                   gms.move_arm('look_at_workspace'),
                                   transitions={
                                       'succeeded': 'SELECT_OBJECT_TO_PLACE',
                                       'failed':
                                       'MOVE_ARM_TO_INTERMEDIATE_POSE'
                                   })

            #normal placing if arm and base alignment pipeline fails
            smach.StateMachine.add(
                'STOP_ALL_COMPONENTS',
                gbs.send_event([
                    ('/gripper_to_object_pose_error_calculator/event_in',
                     'e_stop'), ('/pregrasp_arm_monitor/event_in', 'e_stop'),
                    ('/mcr_common/relative_displacement_calculator/event_in',
                     'e_stop'),
                    ('/mcr_navigation/relative_base_controller/event_in',
                     'e_stop'), ('/planned_motion/event_in', 'e_stop')
                ]),
                transitions={'success': 'SELECT_OBJECT_TO_PLACE'})
    def __init__(self, use_mockup=None):
        smach.StateMachine.__init__(
            self,
            outcomes=[
                "pose_skipped_but_platform_limit_reached",
                "no_more_free_poses",
                "no_more_free_poses_at_robot_platf",
                "no_more_task_for_given_type",
            ],
            input_keys=[
                "base_pose_to_approach",
                "desired_distance_to_workspace",
                "found_objects",
                "lasttask",
                "move_arm_to",
                "move_base_by",
                "next_arm_pose_index",
                "object_pose",
                "object_to_be_adjust_to",
                "object_to_grasp",
                "objects_to_be_grasped",
                "prev_vs_result",
                "rear_platform_free_poses",
                "rear_platform_occupied_poses",
                "recognized_objects",
                "source_visits",
                "task_list",
                "test",
                "vscount",
            ],
            output_keys=[
                "base_pose_to_approach",
                "found_objects",
                "lasttask",
                "move_arm_to",
                "move_base_by",
                "next_arm_pose_index",
                "object_to_be_adjust_to",
                "object_to_grasp",
                "objects_to_be_grasped",
                "prev_vs_result",
                "rear_platform_free_poses",
                "rear_platform_occupied_poses",
                "source_visits",
                "task_list",
                "test",
                "vscount",
            ],
        )

        self.use_mockup = use_mockup
        self.use_visual_servoing = True

        with self:
            smach.StateMachine.add(
                "SELECT_SOURCE_SUBTASK",
                btts.select_btt_subtask(type="source"),
                transitions={
                    "task_selected": "MOVE_TO_SOURCE_LOCATION_SAFE",
                    "no_more_task_for_given_type":
                    "no_more_task_for_given_type",
                },
            )

            # required before any call to gns.approach_pose, moves arm within footprint
            smach.StateMachine.add(
                "MOVE_TO_SOURCE_LOCATION_SAFE",
                gms.move_arm("look_at_workspace"),
                transitions={
                    "succeeded": "MOVE_TO_SOURCE_LOCATION",
                    "failed": "MOVE_TO_SOURCE_LOCATION_SAFE",
                },
            )

            smach.StateMachine.add(
                "MOVE_TO_SOURCE_LOCATION",
                gns.approach_pose(),
                transitions={
                    "succeeded": "STOP_ALL_COMPONENTS_AT_START",
                    "failed": "MOVE_TO_SOURCE_LOCATION",
                },
            )

            smach.StateMachine.add(
                "STOP_ALL_COMPONENTS_AT_START",
                gbs.send_event([
                    (
                        "/gripper_to_object_pose_error_calculator/event_in",
                        "e_stop",
                    ),
                    (
                        "/mcr_common/relative_displacement_calculator/event_in",
                        "e_stop",
                    ),
                    ("/pregrasp_planner/event_in", "e_stop"),
                    (
                        "/mcr_navigation/relative_base_controller/event_in",
                        "e_stop",
                    ),
                    ("/planned_motion/event_in", "e_stop"),
                ]),
                transitions={"success": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE"},
            )

            smach.StateMachine.add(
                "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
                gns.adjust_to_workspace(0.2),
                transitions={
                    "succeeded": "SELECT_NEXT_LOOK_POSE",
                    "failed": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
                },
            )

            smach.StateMachine.add(
                "SELECT_NEXT_LOOK_POSE",
                gms.select_arm_pose([
                    "look_at_workspace_right",
                    "look_at_workspace",
                    "look_at_workspace_left",
                ]),
                transitions={
                    "succeeded": "LOOK_AROUND",
                    "failed": "RECOGNIZE_OBJECTS",
                },
            )

            smach.StateMachine.add(
                "LOOK_AROUND",
                gms.move_arm(),
                transitions={
                    "succeeded": "RECOGNIZE_OBJECTS",
                    "failed": "LOOK_AROUND",
                },
            )

            smach.StateMachine.add(
                "RECOGNIZE_OBJECTS",
                gps.find_objects(retries=1),
                transitions={
                    "objects_found": "ACCUMULATE_RECOGNIZED_LISTS",
                    "no_objects_found": "ACCUMULATE_RECOGNIZED_LISTS",
                },
                remapping={"found_objects": "recognized_objects"},
            )

            smach.StateMachine.add(
                "ACCUMULATE_RECOGNIZED_LISTS",
                gps.accumulate_recognized_objects_list(),
                transitions={
                    "complete": "SELECT_OBJECT_TO_BE_GRASPED",
                    "merged": "SELECT_NEXT_LOOK_POSE",
                },
            )

            smach.StateMachine.add(
                "SELECT_OBJECT_TO_BE_GRASPED",
                btts.select_object_to_be_grasped(),
                transitions={
                    "obj_selected":
                    "COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
                    "no_obj_selected":
                    "SKIP_SOURCE_POSE",
                    "no_more_free_poses_at_robot_platf":
                    "no_more_free_poses_at_robot_platf",
                },
            )

            smach.StateMachine.add(
                "COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
                gbs.send_event([
                    ("/pregrasp_planner/event_in", "e_start"),
                    (
                        "/gripper_to_object_pose_error_calculator/event_in",
                        "e_start",
                    ),
                    (
                        "/mcr_common/relative_displacement_calculator/event_in",
                        "e_start",
                    ),
                ]),
                transitions={
                    "success": "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT"
                },
            )

            smach.StateMachine.add(
                "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
                gbs.wait_for_events([
                    ("/pregrasp_planner/event_out", "e_success", True),
                    (
                        "/gripper_to_object_pose_error_calculator/event_out",
                        "e_success",
                        True,
                    ),
                    (
                        "/mcr_common/relative_displacement_calculator/event_out",
                        "e_done",
                        True,
                    ),
                ]),
                transitions={
                    "success":
                    "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
                    "timeout":
                    "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
                    "failure":
                    "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
                },
            )

            smach.StateMachine.add(
                "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
                gbs.send_event([
                    (
                        "/gripper_to_object_pose_error_calculator/event_in",
                        "e_stop",
                    ),
                    ("/pregrasp_planner/event_in", "e_stop"),
                    (
                        "/mcr_common/relative_displacement_calculator/event_in",
                        "e_stop",
                    ),
                ]),
                transitions={"success": "MOVE_ARM_AND_BASE_TO_OBJECT"},
            )

            smach.StateMachine.add(
                "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
                gbs.send_event([
                    (
                        "/gripper_to_object_pose_error_calculator/event_in",
                        "e_stop",
                    ),
                    ("/pregrasp_planner/event_in", "e_stop"),
                    (
                        "/mcr_common/relative_displacement_calculator/event_in",
                        "e_stop",
                    ),
                ]),
                transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"},
            )

            smach.StateMachine.add(
                "MOVE_ARM_AND_BASE_TO_OBJECT",
                gbs.send_event([
                    (
                        "/mcr_navigation/relative_base_controller/event_in",
                        "e_start",
                    ),
                    ("/planned_motion/event_in", "e_start"),
                ]),
                transitions={"success": "WAIT_ARM_AND_BASE_TO_OBJECT"},
            )

            smach.StateMachine.add(
                "WAIT_ARM_AND_BASE_TO_OBJECT",
                gbs.wait_for_events([
                    (
                        "/mcr_navigation/relative_base_controller/event_out",
                        "e_done",
                        True,
                    ),
                    (
                        "/mcr_navigation/collision_velocity_filter/event_out",
                        "e_zero_velocities_forwarded",
                        False,
                    ),
                    ("/planned_motion/event_out", "e_success", True),
                ]),
                transitions={
                    "success": "STOP_MOVE_ARM_BASE_TO_OBJECT",
                    "timeout": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
                    "failure": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
                },
            )

            smach.StateMachine.add(
                "STOP_MOVE_ARM_BASE_TO_OBJECT",
                gbs.send_event([
                    (
                        "/mcr_navigation/relative_base_controller/event_in",
                        "e_stop",
                    ),
                    ("/planned_motion/event_in", "e_stop"),
                ]),
                #                transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing
                transitions={"success": "GRASP_OBJ"},
            )  # comment out if using visual servoing

            smach.StateMachine.add(
                "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
                gbs.send_event([
                    (
                        "/mcr_navigation/relative_base_controller/event_in",
                        "e_stop",
                    ),
                    ("/planned_motion/event_in", "e_stop"),
                ]),
                transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"},
            )

            smach.StateMachine.add(
                "ALIGN_WITH_OBJECT_LOOP",
                mir_gbs.loop_for_vs(1),
                transitions={
                    "loop": "START_VISUAL_SERVOING",
                    "continue": "GRASP_OBJ",
                },
            )

            smach.StateMachine.add(
                "START_VISUAL_SERVOING",
                gbs.send_event([("/mir_controllers/visual_servoing/event_in",
                                 "e_start")]),
                transitions={"success": "WAIT_VISUAL_SERVOING"},
            )

            smach.StateMachine.add(
                "WAIT_VISUAL_SERVOING",
                gbs.wait_for_events(
                    [(
                        "/mcr_monitoring/component_wise_pose_error_monitor/event_out",
                        "e_stop",
                        True,
                    )],
                    timeout_duration=30,
                ),
                transitions={
                    "success": "STOP_VISUAL_SERVOING",
                    "timeout": "STOP_VISUAL_SERVOING_WITH_FAILURE",
                    "failure": "STOP_VISUAL_SERVOING_WITH_FAILURE",
                },
            )

            smach.StateMachine.add(
                "STOP_VISUAL_SERVOING",
                gbs.send_event([("/mir_controllers/visual_servoing/event_in",
                                 "e_stop")]),
                transitions={"success": "SET_VISUAL_SERVOING_SUCCESS"},
            )

            smach.StateMachine.add(
                "STOP_VISUAL_SERVOING_WITH_FAILURE",
                gbs.send_event([("/mir_controllers/visual_servoing/event_in",
                                 "e_stop")]),
                transitions={"success": "SET_VISUAL_SERVOING_FAILURE"},
            )

            smach.StateMachine.add(
                "SET_VISUAL_SERVOING_SUCCESS",
                mir_gbs.set_vs_status(status=True),
                transitions={"success": "OPEN_GRIPPER_FOR_APPROACH_OBJECT"},
            )

            smach.StateMachine.add(
                "SET_VISUAL_SERVOING_FAILURE",
                mir_gbs.set_vs_status(status=False),
                transitions={"success": "SELECT_OBJECT_TO_BE_GRASPED"},
            )

            smach.StateMachine.add(
                "OPEN_GRIPPER_FOR_APPROACH_OBJECT",
                gms.control_gripper("open"),
                transitions={"succeeded": "APPROACH_OBJECT"},
            )

            smach.StateMachine.add(
                "APPROACH_OBJECT",
                gbs.send_event([(
                    "/guarded_approach_pose/coordinator/event_in",
                    "e_start",
                )]),
                transitions={"success": "WAIT_APPROACH_OBJECT"},
            )

            smach.StateMachine.add(
                "WAIT_APPROACH_OBJECT",
                gbs.wait_for_events(
                    [(
                        "/guarded_approach_pose/coordinator/event_out",
                        "e_success",
                        True,
                    )],
                    timeout_duration=6,
                ),
                transitions={
                    "success": "STOP_APPROACH_OBJECT",
                    "timeout": "STOP_APPROACH_OBJECT",
                    "failure": "STOP_APPROACH_OBJECT",
                },
            )

            smach.StateMachine.add(
                "STOP_APPROACH_OBJECT",
                gbs.send_event([("/guarded_approach_pose/coordinator/event_in",
                                 "e_stop")]),
                transitions={"success": "CLOSE_GRIPPER"},
            )

            smach.StateMachine.add(
                "CLOSE_GRIPPER",
                gms.control_gripper("close"),
                transitions={"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM"},
            )

            smach.StateMachine.add(
                "GRASP_OBJ",
                gms.linear_motion(operation="grasp"),
                transitions={
                    "succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
                    "failed": "DELETE_FROM_RECOGNIZED_OBJECTS",
                },
            )  # TODO: this could loop

            smach.StateMachine.add(
                "GRASP_OBJ_VISUAL_SERVOING",
                gms.linear_motion(operation="grasp", offset_x=-0.05),
                transitions={
                    "succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
                    "failed": "DELETE_FROM_RECOGNIZED_OBJECTS",
                },
            )  # TODO: this could loop

            smach.StateMachine.add(
                "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
                btts.pre_place_obj_on_rear_platform_btt(),
                transitions={
                    "succeeded": "VERIFY_GRASPED",
                    "no_more_free_poses": "no_more_free_poses",
                },
            )

            smach.StateMachine.add(
                "VERIFY_GRASPED",
                gbs.send_event([(
                    "/gripper_controller/grasp_monitor/event_in",
                    "e_trigger",
                )]),
                transitions={"success": "WAIT_FOR_VERIFY_GRASPED"},
            )

            smach.StateMachine.add(
                "WAIT_FOR_VERIFY_GRASPED",
                gbs.wait_for_events(
                    [(
                        "/gripper_controller/grasp_monitor/event_out",
                        "e_object_grasped",
                        True,
                    )],
                    timeout_duration=5,
                ),
                transitions={
                    "success": "PLACE_OBJ_ON_REAR_PLATFORM",
                    "timeout": "OPEN_GRIPPER_FOR_GRASP_FAILURE",
                    "failure": "OPEN_GRIPPER_FOR_GRASP_FAILURE",
                },
            )

            smach.StateMachine.add(
                "OPEN_GRIPPER_FOR_GRASP_FAILURE",
                gms.control_gripper("open"),
                transitions={"succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS"},
            )

            # THESE ARE NEVER ENTERED?
            # It would mean we've grasped the object - and don't have somewhere to place it.
            # If we don't have somewhere to place it, we won't grasp in the first place?
            smach.StateMachine.add(
                "PLACE_OBJ_ON_REAR_PLATFORM",
                btts.place_obj_on_rear_platform_btt(),
                transitions={
                    "succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS",
                    "no_more_free_poses": "no_more_free_poses",
                },
            )

            smach.StateMachine.add(
                "DELETE_FROM_RECOGNIZED_OBJECTS",
                btts.delete_from_recognized_objects(),
                transitions={"succeeded": "SELECT_OBJECT_TO_BE_GRASPED"},
                remapping={"object_to_delete": "object_to_grasp"},
            )

            smach.StateMachine.add(
                "ADJUST_POSE_WRT_WORKSPACE_NEXT",
                gns.adjust_to_workspace(0.1),
                transitions={
                    "succeeded": "SELECT_OBJECT_TO_BE_GRASPED",
                    "failed": "ADJUST_POSE_WRT_WORKSPACE_NEXT",
                },
            )

            smach.StateMachine.add(
                "NO_MORE_FREE_POSES_SAFE",
                gms.move_arm("look_at_workspace"),
                transitions={
                    "succeeded": "no_more_free_poses",
                    "failed": "SKIP_SOURCE_POSE_SAFE",
                },
            )

            # MISC STATES
            smach.StateMachine.add(
                "SKIP_SOURCE_POSE_SAFE",
                gms.move_arm("look_at_workspace"),
                transitions={
                    "succeeded": "SKIP_SOURCE_POSE",
                    "failed": "SKIP_SOURCE_POSE_SAFE",
                },
            )

            smach.StateMachine.add(
                "SKIP_SOURCE_POSE",
                btts.skip_pose("source"),
                transitions={
                    "pose_skipped":
                    "SELECT_SOURCE_SUBTASK",
                    "pose_skipped_but_platform_limit_reached":
                    "pose_skipped_but_platform_limit_reached",
                },
            )
    def __init__(self, use_mockup=None):
        smach.StateMachine.__init__(self, outcomes=['pose_skipped_but_platform_limit_reached',
                                                    'no_more_free_poses',
                                                    'no_more_free_poses_at_robot_platf',
                                                    'no_more_task_for_given_type'],
                                          input_keys=['base_pose_to_approach',
                                                      'desired_distance_to_workspace',
                                                      'found_objects',
                                                      'lasttask',
                                                      'move_arm_to',
                                                      'move_base_by',
						                              'next_arm_pose_index',
                                                      'object_pose',
                                                      'object_to_be_adjust_to',
                                                      'object_to_grasp',
                                                      'objects_to_be_grasped',
                                                      'prev_vs_result',
                                                      'rear_platform_free_poses',
                                                      'rear_platform_occupied_poses',
                                                      'recognized_objects',
                                                      'source_visits',
                                                      'task_list',
						                              'test',
                                                      'vscount'],
                                          output_keys=['base_pose_to_approach',
                                                       'found_objects',
                                                       'lasttask',
                                                       'move_arm_to',
                                                       'move_base_by',
						                               'next_arm_pose_index',
                                                       'object_to_be_adjust_to',
                                                       'object_to_grasp',
                                                       'objects_to_be_grasped',
                                                       'prev_vs_result',
                                                       'rear_platform_free_poses',
                                                       'rear_platform_occupied_poses',
                                                       'source_visits',
                                                       'task_list',
						                               'test',
                                                       'vscount'])

        self.use_mockup = use_mockup
        self.use_visual_servoing = True

        with self:
            smach.StateMachine.add('SELECT_SOURCE_SUBTASK', btts.select_btt_subtask(type="source"),
                transitions={'task_selected': 'MOVE_TO_SOURCE_LOCATION_SAFE',
                             'no_more_task_for_given_type': 'no_more_task_for_given_type'})

            # required before any call to gns.approach_pose, moves arm within footprint
            smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION_SAFE', gms.move_arm('look_at_workspace'),
                transitions={'succeeded': 'MOVE_TO_SOURCE_LOCATION',
                             'failed': 'MOVE_TO_SOURCE_LOCATION_SAFE'})

            smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION', gns.approach_pose(),
                transitions={'succeeded': 'STOP_ALL_COMPONENTS_AT_START',
                             'failed': 'MOVE_TO_SOURCE_LOCATION'})

            smach.StateMachine.add('STOP_ALL_COMPONENTS_AT_START', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
                                                                          ('/mcr_common/relative_displacement_calculator/event_in','e_stop'),
                                                                          ('/pregrasp_planner/event_in','e_stop'),
                                                                          ('/mcr_navigation/relative_base_controller/event_in','e_stop'),
                                                                          ('/planned_motion/event_in','e_stop')]),
                transitions={'success':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'})


            smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE', gns.adjust_to_workspace(0.2),
                transitions={'succeeded':'SELECT_NEXT_LOOK_POSE',
                             'failed':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'})

            smach.StateMachine.add('SELECT_NEXT_LOOK_POSE', gms.select_arm_pose(['look_at_workspace_right', 'look_at_workspace','look_at_workspace_left']),
                    transitions={'succeeded': 'LOOK_AROUND',
                    'failed': 'RECOGNIZE_OBJECTS'})

            smach.StateMachine.add('LOOK_AROUND', gms.move_arm(),
                transitions={'succeeded': 'RECOGNIZE_OBJECTS',
                             'failed': 'LOOK_AROUND'})

            smach.StateMachine.add('RECOGNIZE_OBJECTS', gps.find_objects(retries=1),
                transitions={'objects_found': 'ACCUMULATE_RECOGNIZED_LISTS',
                            'no_objects_found':'ACCUMULATE_RECOGNIZED_LISTS'},
                remapping={'found_objects':'recognized_objects'})

            smach.StateMachine.add('ACCUMULATE_RECOGNIZED_LISTS', gps.accumulate_recognized_objects_list(),
                transitions={'complete': 'SELECT_OBJECT_TO_BE_GRASPED',
                             'merged':'SELECT_NEXT_LOOK_POSE'})

            smach.StateMachine.add('SELECT_OBJECT_TO_BE_GRASPED', btts.select_object_to_be_grasped(),
                transitions={'obj_selected':'COMPUTE_ARM_BASE_SHIFT_TO_OBJECT',
                            'no_obj_selected':'SKIP_SOURCE_POSE',
                            'no_more_free_poses_at_robot_platf':'no_more_free_poses_at_robot_platf'})

            smach.StateMachine.add('COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/pregrasp_planner/event_in','e_start'),
                                                                                       ('/gripper_to_object_pose_error_calculator/event_in','e_start'),
                                                                                       ('/mcr_common/relative_displacement_calculator/event_in','e_start')]),
                transitions={'success':'WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT'})

            smach.StateMachine.add('WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.wait_for_events([('/pregrasp_planner/event_out','e_success', True),
                                                                                                 ('/gripper_to_object_pose_error_calculator/event_out','e_success', True),
                                                                                                 ('/mcr_common/relative_displacement_calculator/event_out','e_done', True)]),
                transitions={'success':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT',
                             'timeout': 'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE',
                             'failure':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE'})

            smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
                                                                                            ('/pregrasp_planner/event_in','e_stop'),
                                                                                            ('/mcr_common/relative_displacement_calculator/event_in','e_stop')]),
                transitions={'success':'MOVE_ARM_AND_BASE_TO_OBJECT'})

            smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
                                                                                            ('/pregrasp_planner/event_in','e_stop'),
                                                                                            ('/mcr_common/relative_displacement_calculator/event_in','e_stop')]),
                transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'})

            smach.StateMachine.add('MOVE_ARM_AND_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_start'),
                                                                                  ('/planned_motion/event_in', 'e_start')]),
                transitions={'success':'WAIT_ARM_AND_BASE_TO_OBJECT'})

            smach.StateMachine.add('WAIT_ARM_AND_BASE_TO_OBJECT', gbs.wait_for_events([('/mcr_navigation/relative_base_controller/event_out','e_done', True),
                                                                                       ('/mcr_navigation/collision_velocity_filter/event_out','e_zero_velocities_forwarded', False),
                                                                                       ('/planned_motion/event_out', 'e_success', True)]),
                transitions={'success':'STOP_MOVE_ARM_BASE_TO_OBJECT',
                             'timeout': 'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE',
                             'failure':'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE'})

            smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'),
                                                                                   ('/planned_motion/event_in','e_stop')]),
#                transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing
                transitions={'success':'GRASP_OBJ'}) # comment out if using visual servoing

            smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'),
                                                                                                ('/planned_motion/event_in','e_stop')]),
                transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'})

            smach.StateMachine.add('ALIGN_WITH_OBJECT_LOOP', mir_gbs.loop_for_vs(1),
                                      transitions={'loop': 'START_VISUAL_SERVOING',
                                                   'continue': 'GRASP_OBJ'})

            smach.StateMachine.add('START_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_start')]),
                transitions={'success':'WAIT_VISUAL_SERVOING'})

            smach.StateMachine.add('WAIT_VISUAL_SERVOING', gbs.wait_for_events([('/mcr_monitoring/component_wise_pose_error_monitor/event_out','e_stop', True)], timeout_duration=30),
                transitions={'success':'STOP_VISUAL_SERVOING',
                             'timeout': 'STOP_VISUAL_SERVOING_WITH_FAILURE',
                             'failure':'STOP_VISUAL_SERVOING_WITH_FAILURE'})

            smach.StateMachine.add('STOP_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]),
                transitions={'success':'SET_VISUAL_SERVOING_SUCCESS'})

            smach.StateMachine.add('STOP_VISUAL_SERVOING_WITH_FAILURE', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]),
                transitions={'success':'SET_VISUAL_SERVOING_FAILURE'})

            smach.StateMachine.add('SET_VISUAL_SERVOING_SUCCESS', mir_gbs.set_vs_status(status=True),
                transitions={'success':'OPEN_GRIPPER_FOR_APPROACH_OBJECT'})

            smach.StateMachine.add('SET_VISUAL_SERVOING_FAILURE', mir_gbs.set_vs_status(status=False),
                transitions={'success':'SELECT_OBJECT_TO_BE_GRASPED'})

            smach.StateMachine.add('OPEN_GRIPPER_FOR_APPROACH_OBJECT', gms.control_gripper('open'),
                transitions={'succeeded':'APPROACH_OBJECT'})

            smach.StateMachine.add('APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_start')]),
                transitions={'success':'WAIT_APPROACH_OBJECT'})

            smach.StateMachine.add('WAIT_APPROACH_OBJECT', gbs.wait_for_events([('/guarded_approach_pose/coordinator/event_out','e_success', True)], timeout_duration=6),
                transitions={'success':'STOP_APPROACH_OBJECT',
                             'timeout': 'STOP_APPROACH_OBJECT',
                             'failure':'STOP_APPROACH_OBJECT'})

            smach.StateMachine.add('STOP_APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_stop')]),
                transitions={'success':'CLOSE_GRIPPER'})

            smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'),
                transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM'})

            smach.StateMachine.add('GRASP_OBJ', gms.linear_motion(operation='grasp'),
                transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM',
                             'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop

            smach.StateMachine.add('GRASP_OBJ_VISUAL_SERVOING', gms.linear_motion(operation='grasp', offset_x=-0.05),
                transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM',
                             'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop

            smach.StateMachine.add('PRE_PLACE_OBJ_ON_REAR_PLATFORM', btts.pre_place_obj_on_rear_platform_btt(),
                transitions={'succeeded':'VERIFY_GRASPED',
                             'no_more_free_poses':'no_more_free_poses'})

            smach.StateMachine.add('VERIFY_GRASPED', gbs.send_event([('/gripper_controller/grasp_monitor/event_in', 'e_trigger')]),
                transitions={'success':'WAIT_FOR_VERIFY_GRASPED'})

            smach.StateMachine.add('WAIT_FOR_VERIFY_GRASPED', gbs.wait_for_events([('/gripper_controller/grasp_monitor/event_out','e_object_grasped', True)], timeout_duration=5),
                transitions={'success':'PLACE_OBJ_ON_REAR_PLATFORM',
                             'timeout':'OPEN_GRIPPER_FOR_GRASP_FAILURE',
                             'failure':'OPEN_GRIPPER_FOR_GRASP_FAILURE'})

            smach.StateMachine.add('OPEN_GRIPPER_FOR_GRASP_FAILURE', gms.control_gripper('open'),
                transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS'})

            # THESE ARE NEVER ENTERED?
            # It would mean we've grasped the object - and don't have somewhere to place it.
            # If we don't have somewhere to place it, we won't grasp in the first place?
            smach.StateMachine.add('PLACE_OBJ_ON_REAR_PLATFORM', btts.place_obj_on_rear_platform_btt(),
                transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS',
                             'no_more_free_poses':'no_more_free_poses'})

            smach.StateMachine.add('DELETE_FROM_RECOGNIZED_OBJECTS', btts.delete_from_recognized_objects(),
                transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED'},
                remapping={'object_to_delete': 'object_to_grasp'})

            smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_NEXT', gns.adjust_to_workspace(0.1),
                transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED',
                             'failed':'ADJUST_POSE_WRT_WORKSPACE_NEXT'})

            smach.StateMachine.add('NO_MORE_FREE_POSES_SAFE',gms.move_arm('look_at_workspace'),
                transitions={'succeeded':'no_more_free_poses',
                             'failed':'SKIP_SOURCE_POSE_SAFE'})

		            # MISC STATES
            smach.StateMachine.add('SKIP_SOURCE_POSE_SAFE', gms.move_arm('look_at_workspace'),
                transitions={'succeeded':'SKIP_SOURCE_POSE',
                             'failed':'SKIP_SOURCE_POSE_SAFE'})

            smach.StateMachine.add('SKIP_SOURCE_POSE', btts.skip_pose('source'),
                transitions={'pose_skipped':'SELECT_SOURCE_SUBTASK',
                             'pose_skipped_but_platform_limit_reached':'pose_skipped_but_platform_limit_reached'})
    def __init__(self):
        smach.StateMachine.__init__(
            self,
            outcomes=["succeeded", "failed", "no_object_for_ppt_platform"],
            input_keys=[
                "base_pose_to_approach",
                "last_grasped_obj",
                "move_arm_to",
                "move_base_by",
                "object_pose",
                "rear_platform_free_poses",
                "rear_platform_occupied_poses",
                "selected_objects",
                "next_arm_pose_index",
                "cavity_pose",
                "task_list",
            ],
            output_keys=[
                "base_pose_to_approach",
                "last_grasped_obj",
                "move_arm_to",
                "move_base_by",
                "rear_platform_free_poses",
                "rear_platform_occupied_poses",
                "selected_objects",
                "next_arm_pose_index",
                "cavity_pose",
                "task_list",
            ],
        )

        with self:

            # TODO: ENSURE WE HAVE ENOUGH SPACE IN THE ARENA
            smach.StateMachine.add(
                "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
                gns.adjust_to_workspace(0.2),
                transitions={
                    "succeeded": "SELECT_OBJECTS_TO_PLACE",
                    "failed": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
                },
            )

            # Select object from task list which needs to be place in cavities.
            smach.StateMachine.add(
                "SELECT_OBJECTS_TO_PLACE",
                ppts.select_objects_to_place(),
                transitions={
                    "objects_selected": "CLEAR_CAVITIES",
                    "no_more_obj_for_this_workspace":
                    "no_object_for_ppt_platform",
                },
            )

            smach.StateMachine.add(
                "CLEAR_CAVITIES",
                ppts.clear_cavities(),
                transitions={"succeeded": "SELECT_NEXT_LOOK_POSE"},
            )

            # CAVITY RECOGNITION PIPELINE
            smach.StateMachine.add(
                "SELECT_NEXT_LOOK_POSE",
                gms.select_arm_pose([
                    "look_at_workspace_right",
                    "look_at_workspace",
                    "look_at_workspace_left",
                ]),
                transitions={
                    "succeeded": "LOOK_AROUND",
                    "failed": "CHECK_FOUND_CAVITIES",
                },
            )

            smach.StateMachine.add(
                "LOOK_AROUND",
                gms.move_arm(),
                transitions={
                    "succeeded": "FIND_CAVITIES_LOOP",
                    "failed": "LOOK_AROUND",
                },
            )

            smach.StateMachine.add(
                "FIND_CAVITIES_LOOP",
                gbs.loop_for(3),
                transitions={
                    "loop": "FIND_CAVITIES",
                    "continue": "CHECK_FOUND_CAVITIES",
                },
            )

            smach.StateMachine.add(
                "FIND_CAVITIES",
                gps.find_cavities(),
                transitions={
                    "succeeded": "TRANSFORM_CAVITIES",
                    "failed": "SELECT_NEXT_LOOK_POSE",
                },
            )

            smach.StateMachine.add(
                "TRANSFORM_CAVITIES",
                gps.transform_object_poses(frame_id="/odom"),
                transitions={"succeeded": "FIND_BEST_MATCHED_CAVITIES"},
                remapping={"found_objects": "found_cavities"},
            )

            smach.StateMachine.add(
                "FIND_BEST_MATCHED_CAVITIES",
                gps.find_best_matched_cavities(),
                transitions={
                    "succeeded": "SELECT_NEXT_LOOK_POSE",
                    "complete": "CHECK_FOUND_CAVITIES",
                },
            )

            smach.StateMachine.add(
                "CHECK_FOUND_CAVITIES",
                gps.check_found_cavities(),
                transitions={
                    "cavities_found": "STOP_ALL_COMPONENTS_AT_START",
                    "no_cavities_found": "failed",
                },
            )

            smach.StateMachine.add(
                "STOP_ALL_COMPONENTS_AT_START",
                gbs.send_event([
                    (
                        "/gripper_to_object_pose_error_calculator/event_in",
                        "e_stop",
                    ),
                    (
                        "/mcr_common/relative_displacement_calculator/event_in",
                        "e_stop",
                    ),
                    ("/pregrasp_planner/event_in", "e_stop"),
                    ("/pregrasp_arm_monitor/event_in", "e_stop"),
                    (
                        "/mcr_navigation/relative_base_controller/event_in",
                        "e_stop",
                    ),
                    ("/planned_motion/event_in", "e_stop"),
                ]),
                transitions={"success": "SELECT_OBJECT_TO_PLACE"},
            )

            # OBJECT PLACING pipeline
            # select cavitiy based on recognized cavities and list of objects to place in cavity.
            smach.StateMachine.add(
                "SELECT_OBJECT_TO_PLACE",
                ppts.select_object_to_place(),
                transitions={
                    "object_selected": "COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
                    "no_more_objects": "succeeded",
                    "no_more_cavities": "succeeded",
                },
                remapping={"found_objects": "best_matched_cavities"},
            )

            # ARM AND BASE ALIGNMENT TO CAVITY PIPELINE
            smach.StateMachine.add(
                "COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
                gbs.send_event([
                    ("/pregrasp_planner/event_in", "e_start"),
                    (
                        "/gripper_to_object_pose_error_calculator/event_in",
                        "e_start",
                    ),
                    (
                        "/mcr_common/relative_displacement_calculator/event_in",
                        "e_start",
                    ),
                ]),
                transitions={
                    "success": "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY"
                },
            )

            smach.StateMachine.add(
                "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
                gbs.wait_for_events([
                    ("/pregrasp_planner/event_out", "e_success", True),
                    (
                        "/gripper_to_object_pose_error_calculator/event_out",
                        "e_success",
                        True,
                    ),
                    (
                        "/mcr_common/relative_displacement_calculator/event_out",
                        "e_done",
                        True,
                    ),
                ]),
                transitions={
                    "success": "STOP_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
                    "timeout":
                    "STOP_ALL_COMPONENTS",  # should we try again for compute arm base shift
                    "failure": "STOP_ALL_COMPONENTS",
                },
            )  # should we try again for compute arm base shift

            smach.StateMachine.add(
                "STOP_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
                gbs.send_event([
                    (
                        "/gripper_to_object_pose_error_calculator/event_in",
                        "e_stop",
                    ),
                    ("/pregrasp_planner/event_in", "e_stop"),
                    (
                        "/mcr_common/relative_displacement_calculator/event_in",
                        "e_stop",
                    ),
                ]),
                transitions={"success": "ALIGN_BASE_WITH_CAVITY"},
            )

            smach.StateMachine.add(
                "ALIGN_BASE_WITH_CAVITY",
                gbs.send_event([(
                    "/mcr_navigation/relative_base_controller/event_in",
                    "e_start",
                )]),
                transitions={"success": "WAIT_ALIGN_BASE_WITH_CAVITY"},
            )

            smach.StateMachine.add(
                "WAIT_ALIGN_BASE_WITH_CAVITY",
                gbs.wait_for_events([
                    (
                        "/mcr_navigation/relative_base_controller/event_out",
                        "e_done",
                        True,
                    ),
                    (
                        "/mcr_navigation/collision_velocity_filter/event_out",
                        "e_zero_velocities_forwarded",
                        False,
                    ),
                ]),
                transitions={
                    "success": "STOP_ALIGN_BASE_WITH_CAVITY",
                    "timeout":
                    "STOP_ALL_COMPONENTS",  # should we try again for compute and aligh arm and base to object
                    "failure": "STOP_ALL_COMPONENTS",
                },
            )  # should we try again for compute and aligh arm and base to object

            smach.StateMachine.add(
                "STOP_ALIGN_BASE_WITH_CAVITY",
                gbs.send_event([(
                    "/mcr_navigation/relative_base_controller/event_in",
                    "e_stop",
                )]),
                transitions={"success": "GRASP_OBJECT_FOR_CAVITY_FROM_PLTF"},
            )

            # make sure that gripper is open
            smach.StateMachine.add(
                "GRASP_OBJECT_FOR_CAVITY_FROM_PLTF",
                ppts.grasp_obj_for_hole_from_pltf(),
                transitions={
                    "object_grasped": "MOVE_ARM_TO_CAVITY",
                    "no_more_obj_for_this_workspace":
                    "no_object_for_ppt_platform",
                },
            )

            smach.StateMachine.add(
                "GO_TO_PREGRASP",
                gms.move_arm("pre_grasp"),
                transitions={
                    "succeeded": "MOVE_ARM_TO_CAVITY",
                    "failed": "MOVE_ARM_TO_CAVITY",
                },
            )

            smach.StateMachine.add(
                "MOVE_ARM_TO_CAVITY",
                gbs.send_event([
                    ("/planned_motion/event_in", "e_start"),
                    # smach.StateMachine.add('MOVE_ARM_TO_CAVITY', gbs.send_event([('/planned_motion/event_in', 'e_start')]),
                    ("/pregrasp_arm_monitor/event_in", "e_start"),
                ]),
                transitions={"success": "WAIT_MOVE_ARM_TO_CAVITY"},
            )

            smach.StateMachine.add(
                "WAIT_MOVE_ARM_TO_CAVITY",
                gbs.wait_for_events([("/pregrasp_arm_monitor/event_out",
                                      "e_done", True)]),
                #          smach.StateMachine.add('WAIT_MOVE_ARM_TO_CAVITY', gbs.wait_for_events([('/planned_motion/event_out','e_success', True)]),
                #                                                                             ('/pregrasp_arm_monitor/event_out', 'e_done', True)]),
                transitions={
                    "success": "STOP_MOVE_ARM_TO_CAVITY",
                    "timeout":
                    "STOP_ALL_COMPONENTS",  # should we try again for compute and aligh arm and base to object
                    "failure": "STOP_ALL_COMPONENTS",
                },
            )  # should we try again for compute and aligh arm and base to object

            # smach.StateMachine.add('STOP_MOVE_ARM_TO_CAVITY', gbs.send_event([('/planned_motion/event_in','e_stop')]),
            smach.StateMachine.add(
                "STOP_MOVE_ARM_TO_CAVITY",
                gbs.send_event([
                    ("/planned_motion/event_in", "e_stop"),
                    ("/pregrasp_arm_monitor/event_in", "e_stop"),
                ]),
                transitions={"success": "MOVE_ARM_TO_INTERMEDIATE_POSE"},
            )

            smach.StateMachine.add(
                "PLACE_OBJECT",
                gms.linear_motion(operation="release"),
                transitions={
                    "succeeded": "WIGGLE_ARM_LEFT",
                    "failed": "PLACE_OBJECT",
                },
            )

            smach.StateMachine.add(
                "WIGGLE_ARM_LEFT",
                ppts.ppt_wiggle_arm(wiggle_offset=-0.12),
                transitions={
                    "succeeded": "WIGGLE_ARM_RIGHT",
                    "failed": "WIGGLE_ARM_RIGHT",
                },
            )

            smach.StateMachine.add(
                "WIGGLE_ARM_RIGHT",
                ppts.ppt_wiggle_arm(wiggle_offset=0.24),
                transitions={
                    "succeeded": "MOVE_ARM_TO_INTERMEDIATE_POSE",
                    "failed": "MOVE_ARM_TO_INTERMEDIATE_POSE",
                },
            )

            smach.StateMachine.add(
                "MOVE_ARM_TO_INTERMEDIATE_POSE",
                gms.move_arm("look_at_workspace"),
                transitions={
                    "succeeded": "SELECT_OBJECT_TO_PLACE",
                    "failed": "MOVE_ARM_TO_INTERMEDIATE_POSE",
                },
            )

            # normal placing if arm and base alignment pipeline fails
            smach.StateMachine.add(
                "STOP_ALL_COMPONENTS",
                gbs.send_event([
                    (
                        "/gripper_to_object_pose_error_calculator/event_in",
                        "e_stop",
                    ),
                    ("/pregrasp_arm_monitor/event_in", "e_stop"),
                    (
                        "/mcr_common/relative_displacement_calculator/event_in",
                        "e_stop",
                    ),
                    (
                        "/mcr_navigation/relative_base_controller/event_in",
                        "e_stop",
                    ),
                    ("/planned_motion/event_in", "e_stop"),
                ]),
                transitions={"success": "SELECT_OBJECT_TO_PLACE"},
            )