def create(self):
        pick_group = 'robot1'
        names1 = [
            'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
            'robot1_elbow_joint', 'robot1_wrist_1_joint',
            'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
        ]
        gripper1 = "vacuum_gripper1_suction_cup"
        # x:935 y:528, x:36 y:516
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'])
        _state_machine.userdata.part_pose = []
        _state_machine.userdata.pick_configuration = []

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:76 y:45
            OperatableStateMachine.add('Move Robot 1 Home',
                                       SrdfStateToMoveit(
                                           config_name='R1Home',
                                           move_group=pick_group,
                                           action_topic='/move_group',
                                           robot_name=""),
                                       transitions={
                                           'reached': 'Feed part',
                                           'planning_failed': 'failed',
                                           'control_failed': 'failed',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name',
                                           'move_group': 'move_group',
                                           'robot_name': 'robot_name',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:584 y:175
            OperatableStateMachine.add('Compute pick',
                                       ComputeGraspState(group=pick_group,
                                                         offset=0.0,
                                                         joint_names=names1,
                                                         tool_link=gripper1),
                                       transitions={
                                           'continue': 'Activate gripper',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'part_pose',
                                           'joint_values':
                                           'pick_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:677 y:255
            OperatableStateMachine.add('Activate gripper',
                                       VacuumGripperControlState(
                                           enable=True,
                                           service_name='/gripper1/control'),
                                       transitions={
                                           'continue': 'Move to pick',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:738 y:336
            OperatableStateMachine.add(
                'Move to pick',
                MoveitToJointsDynState(move_group=pick_group,
                                       offset=0.0,
                                       tool_link=gripper1,
                                       action_topic='/move_group'),
                transitions={
                    'reached': 'Move Robot 1 Home_2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'pick_configuration',
                    'joint_names': 'joint_names'
                })

            # x:825 y:418
            OperatableStateMachine.add('Move Robot 1 Home_2',
                                       SrdfStateToMoveit(
                                           config_name='R1Home',
                                           move_group=pick_group,
                                           action_topic='/move_group',
                                           robot_name=""),
                                       transitions={
                                           'reached': 'finished',
                                           'planning_failed': 'failed',
                                           'control_failed': 'failed',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name',
                                           'move_group': 'move_group',
                                           'robot_name': 'robot_name',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:269 y:57
            OperatableStateMachine.add('Feed part',
                                       ControlFeederState(activation=True),
                                       transitions={
                                           'succeeded': 'Locate part',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:412 y:112
            OperatableStateMachine.add(
                'Locate part',
                DetectPartCameraState(ref_frame='robot1_base',
                                      camera_topic='/hrwros/logical_camera_1',
                                      camera_frame='logical_camera_1_frame'),
                transitions={
                    'continue': 'Compute pick',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'part_pose'})

        return _state_machine
Exemple #2
0
    def create(self):
        names1 = [
            'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
            'robot1_elbow_joint', 'robot1_wrist_1_joint',
            'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
        ]
        pick1_group = 'robot1'
        robot1_loc = Pose2D(x=3.8, y=2.1, theta=-90.0)
        gripper1 = "vacuum_gripper1_suction_cup"
        pick2_group = 'robot2'
        robot1_loc_o = Pose2D(x=-4.3, y=-0.9, theta=0.0)
        names2 = [
            'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
            'robot2_elbow_joint', 'robot2_wrist_1_joint',
            'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
        ]
        gripper2 = "vacuum_gripper2_suction_cup"
        # x:208 y:207, x:594 y:345
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'])
        _state_machine.userdata.part_pose = []
        _state_machine.userdata.robot1_loc = robot1_loc
        _state_machine.userdata.pose_turtlebot = []
        _state_machine.userdata.pick1_configuration = []
        _state_machine.userdata.place1_configuration = []
        _state_machine.userdata.speed = 100
        _state_machine.userdata.robot1_loc_o = robot1_loc_o

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:32 y:56
            OperatableStateMachine.add(
                'Move R1 Home',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R1Home',
                    move_group=pick1_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Start Conveyor',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1122 y:138
            OperatableStateMachine.add('Compute pick',
                                       ComputeGraspState(group=pick1_group,
                                                         offset=0.0,
                                                         joint_names=names1,
                                                         tool_link=gripper1,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue': 'Activate Gripper 1',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'part_pose',
                                           'joint_values':
                                           'pick1_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:381 y:59
            OperatableStateMachine.add('Start feeder',
                                       ControlFeederState(activation=True),
                                       transitions={
                                           'succeeded': 'Wait for part',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:718 y:59
            OperatableStateMachine.add('Stop feeder',
                                       ControlFeederState(activation=False),
                                       transitions={
                                           'succeeded': 'Stop Conveyor',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:1104 y:222
            OperatableStateMachine.add('Activate Gripper 1',
                                       VacuumGripperControlState(
                                           enable=True,
                                           service_name='/gripper1/control'),
                                       transitions={
                                           'continue': 'Move R1 to pick',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:971 y:519
            OperatableStateMachine.add('Compute place Turtlebot',
                                       ComputeGraspState(group=pick1_group,
                                                         offset=0.6,
                                                         joint_names=names1,
                                                         tool_link=gripper1,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue': 'Move R1 to place',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'pose_turtlebot',
                                           'joint_values':
                                           'place1_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:1127 y:493
            OperatableStateMachine.add(
                'LocateTurtlebot',
                LocateFactoryDeviceState(model_name='mobile_base',
                                         output_frame_id='world'),
                transitions={
                    'succeeded': 'Compute place Turtlebot',
                    'failed': 'failed'
                },
                autonomy={
                    'succeeded': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'pose_turtlebot'})

            # x:1125 y:370
            OperatableStateMachine.add(
                'Move R1 back Home',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R1Home',
                    move_group=pick1_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Navigate To Robot1',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:5 y:296
            OperatableStateMachine.add(
                'Move R2 back to Home',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R2Home',
                    move_group=pick2_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Move R2 To Bin',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1113 y:59
            OperatableStateMachine.add(
                'Detect Part Camera',
                DetectPartCameraState(ref_frame='robot1_base',
                                      camera_topic='/hrwros/logical_camera_1',
                                      camera_frame='logical_camera_1_frame'),
                transitions={
                    'continue': 'Compute pick',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'part_pose'})

            # x:556 y:60
            OperatableStateMachine.add('Wait for part',
                                       SubscriberState(
                                           topic='/break_beam_sensor_change',
                                           blocking=True,
                                           clear=True),
                                       transitions={
                                           'received': 'Stop feeder',
                                           'unavailable': 'failed'
                                       },
                                       autonomy={
                                           'received': Autonomy.Off,
                                           'unavailable': Autonomy.Off
                                       },
                                       remapping={'message': 'message'})

            # x:1121 y:304
            OperatableStateMachine.add(
                'Move R1 to pick',
                hrwros_factory_states__MoveitToJointsDynState(
                    move_group=pick1_group,
                    offset=0.0,
                    tool_link=gripper1,
                    action_topic='/move_group'),
                transitions={
                    'reached': 'Move R1 back Home',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'pick1_configuration',
                    'joint_names': 'joint_names'
                })

            # x:827 y:519
            OperatableStateMachine.add(
                'Move R1 to place',
                hrwros_factory_states__MoveitToJointsDynState(
                    move_group=pick1_group,
                    offset=0.0,
                    tool_link=gripper1,
                    action_topic='/move_group'),
                transitions={
                    'reached': 'Deactivate Gripper',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'place1_configuration',
                    'joint_names': 'joint_names'
                })

            # x:215 y:107
            OperatableStateMachine.add('Start Conveyor',
                                       SetConveyorPowerState(stop=False),
                                       transitions={
                                           'succeeded': 'Start feeder',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'speed': 'speed'})

            # x:886 y:60
            OperatableStateMachine.add('Stop Conveyor',
                                       SetConveyorPowerState(stop=True),
                                       transitions={
                                           'succeeded': 'Detect Part Camera',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'speed': 'speed'})

            # x:1136 y:432
            OperatableStateMachine.add('Navigate To Robot1',
                                       hrwros_factory_states__MoveBaseState(),
                                       transitions={
                                           'arrived': 'LocateTurtlebot',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'arrived': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'waypoint': 'robot1_loc'})

            # x:661 y:517
            OperatableStateMachine.add('Deactivate Gripper',
                                       VacuumGripperControlState(
                                           enable=False,
                                           service_name='/gripper1/control'),
                                       transitions={
                                           'continue': 'Move R1 Home_2',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:501 y:519
            OperatableStateMachine.add(
                'Move R1 Home_2',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R1Home',
                    move_group=pick1_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Navigate To Robot1_2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:353 y:522
            OperatableStateMachine.add('Navigate To Robot1_2',
                                       hrwros_factory_states__MoveBaseState(),
                                       transitions={
                                           'arrived': 'Move R2 Home',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'arrived': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'waypoint': 'robot1_loc_o'})

            # x:25 y:530
            OperatableStateMachine.add(
                'Detect object near robot2',
                DetectPartCameraState(ref_frame='robot2_base',
                                      camera_topic='/hrwros/logical_camera_2',
                                      camera_frame='logical_camera_2_frame'),
                transitions={
                    'continue': 'Compute pick_2',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'part_pose'})

            # x:42 y:470
            OperatableStateMachine.add('Compute pick_2',
                                       ComputeGraspState(group=pick2_group,
                                                         offset=0.0,
                                                         joint_names=names2,
                                                         tool_link=gripper2,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue':
                                           'Activate Gripper 2 robot2',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'part_pose',
                                           'joint_values':
                                           'pick1_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:20 y:410
            OperatableStateMachine.add('Activate Gripper 2 robot2',
                                       VacuumGripperControlState(
                                           enable=True,
                                           service_name='/gripper2/control'),
                                       transitions={
                                           'continue': 'Move R1 to pick_2',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:211 y:525
            OperatableStateMachine.add(
                'Move R2 Home',
                hrwros_factory_states__SrdfStateToMoveit(
                    config_name='R2Home',
                    move_group=pick2_group,
                    action_topic='/move_group',
                    robot_name=""),
                transitions={
                    'reached': 'Detect object near robot2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:21 y:353
            OperatableStateMachine.add(
                'Move R1 to pick_2',
                hrwros_factory_states__MoveitToJointsDynState(
                    move_group=pick2_group,
                    offset=0.0,
                    tool_link=gripper2,
                    action_topic='/move_group'),
                transitions={
                    'reached': 'Move R2 back to Home',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'pick1_configuration',
                    'joint_names': 'joint_names'
                })

            # x:10 y:220
            OperatableStateMachine.add(
                'Move R2 To Bin',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R2Place',
                    move_group=pick2_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Deactivate R2 Gripper',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:9 y:133
            OperatableStateMachine.add('Deactivate R2 Gripper',
                                       VacuumGripperControlState(
                                           enable=False,
                                           service_name='/gripper2/control'),
                                       transitions={
                                           'continue': 'finished',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

        return _state_machine
Exemple #3
0
    def create(self):
        pick_group = 'robot1'
        names1 = [
            'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
            'robot1_elbow_joint', 'robot1_wrist_1_joint',
            'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
        ]
        gripper1 = "vacuum_gripper1_suction_cup"
        # x:1010 y:559, x:40 y:517
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'])
        _state_machine.userdata.part_pose = []
        _state_machine.userdata.place_configuration = []
        _state_machine.userdata.conveyor_speed = 100
        _state_machine.userdata.joint_names = []
        _state_machine.userdata.pick_configuration = []

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:73 y:34
            OperatableStateMachine.add(
                'Move Robot 1 Home',
                hrwros_factory_states__SrdfStateToMoveit(
                    config_name='R1Home',
                    move_group=pick_group,
                    action_topic='/move_group',
                    robot_name=""),
                transitions={
                    'reached': 'Stars Conveyor Belt',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:673 y:227
            OperatableStateMachine.add('Compute pick',
                                       ComputeGraspState(group=pick_group,
                                                         offset=0.0,
                                                         joint_names=names1,
                                                         tool_link=gripper1,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue': 'Activate gripper',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'part_pose',
                                           'joint_values':
                                           'pick_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:661 y:330
            OperatableStateMachine.add('Activate gripper',
                                       VacuumGripperControlState(
                                           enable=True,
                                           service_name='/gripper1/control'),
                                       transitions={
                                           'continue': 'Move to pick',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:668 y:439
            OperatableStateMachine.add(
                'Move to pick',
                hrwros_factory_states__MoveitToJointsDynState(
                    move_group=pick_group,
                    offset=0.0,
                    tool_link=gripper1,
                    action_topic='/move_group'),
                transitions={
                    'reached': 'Move Robot 1 Home_2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'pick_configuration',
                    'joint_names': 'joint_names'
                })

            # x:668 y:554
            OperatableStateMachine.add(
                'Move Robot 1 Home_2',
                hrwros_factory_states__SrdfStateToMoveit(
                    config_name='R1Home',
                    move_group=pick_group,
                    action_topic='/move_group',
                    robot_name=""),
                transitions={
                    'reached': 'finished',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'place_configuration',
                    'joint_names': 'joint_names'
                })

            # x:507 y:36
            OperatableStateMachine.add('Feed part',
                                       ControlFeederState(activation=True),
                                       transitions={
                                           'succeeded': 'Locate part',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:701 y:31
            OperatableStateMachine.add(
                'Locate part',
                DetectPartCameraState(ref_frame='robot1_base',
                                      camera_topic='/hrwros/logical_camera_1',
                                      camera_frame='logical_camera_1_frame'),
                transitions={
                    'continue': 'Stops Conveyor Belt',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'part_pose'})

            # x:272 y:31
            OperatableStateMachine.add('Deactivate Gripper',
                                       VacuumGripperControlState(
                                           enable=False,
                                           service_name='/gripper1/control'),
                                       transitions={
                                           'continue': 'Feed part',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:93 y:111
            OperatableStateMachine.add('Stars Conveyor Belt',
                                       SetConveyorPowerState(stop=False),
                                       transitions={
                                           'succeeded': 'Deactivate Gripper',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'speed': 'conveyor_speed'})

            # x:665 y:136
            OperatableStateMachine.add('Stops Conveyor Belt',
                                       SetConveyorPowerState(stop=True),
                                       transitions={
                                           'succeeded': 'Compute pick',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'speed': 'conveyor_speed'})

        return _state_machine
    def create(self):
        names1 = [
            'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
            'robot1_elbow_joint', 'robot1_wrist_1_joint',
            'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
        ]
        pick1_group = 'robot1'
        pick2_group = 'robot2'
        robot1_loc = Pose2D(x=-0.2, y=2.03, theta=0.0)
        names2 = [
            'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
            'robot2_elbow_joint', 'robot2_wrist_1_joint',
            'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
        ]
        gripper1 = "vacuum_gripper1_suction_cup"
        gripper2 = "vacuum_gripper2_suction_cup"
        robot2_loc = Pose2D(x=-8.2, y=-1.4, theta=0.0)
        # x:9 y:195, x:413 y:347
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'])
        _state_machine.userdata.part_pose = []
        _state_machine.userdata.speed = '100.0'
        _state_machine.userdata.robot1_loc = robot1_loc
        _state_machine.userdata.pick2_configuration = []
        _state_machine.userdata.pose_turtlebot = []
        _state_machine.userdata.robot2_loc = robot2_loc

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:87 y:217
            OperatableStateMachine.add('Move R2 to Home',
                                       SrdfStateToMoveit(
                                           config_name='R2Home',
                                           move_group=pick2_group,
                                           action_topic='/move_group',
                                           robot_name=''),
                                       transitions={
                                           'reached': 'Move R1 Home',
                                           'planning_failed': 'failed',
                                           'control_failed': 'failed',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name',
                                           'move_group': 'move_group',
                                           'robot_name': 'robot_name',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1025 y:148
            OperatableStateMachine.add('Compute Pick for R1',
                                       ComputeGraspState(group=pick1_group,
                                                         offset=0.0,
                                                         joint_names=names1,
                                                         tool_link=gripper1),
                                       transitions={
                                           'continue': 'Activate Gripper 1',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'part_pose',
                                           'joint_values':
                                           'pick1_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:885 y:309
            OperatableStateMachine.add(
                'Move R1 to Pick',
                MoveitToJointsDynState(move_group=pick1_group,
                                       offset=0.0,
                                       tool_link=gripper1,
                                       action_topic='/move_group'),
                transitions={
                    'reached': 'Move R1 back Home',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'pick1_configuration',
                    'joint_names': 'joint_names'
                })

            # x:421 y:15
            OperatableStateMachine.add('Start feeder',
                                       ControlFeederState(activation=True),
                                       transitions={
                                           'succeeded': 'Wait for part',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:588 y:25
            OperatableStateMachine.add('Stop feeder',
                                       ControlFeederState(activation=False),
                                       transitions={
                                           'succeeded': 'Stop Conveyor',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:845 y:227
            OperatableStateMachine.add('Activate Gripper 1',
                                       VacuumGripperControlState(
                                           enable=True,
                                           service_name='/gripper1/control'),
                                       transitions={
                                           'continue': 'Move R1 to Pick',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:874 y:578
            OperatableStateMachine.add('Compute place Turtlebot',
                                       ComputeGraspState(group=pick1_group,
                                                         offset=0.55,
                                                         joint_names=names1,
                                                         tool_link=gripper1),
                                       transitions={
                                           'continue': 'Move R1 to place',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'pose_turtlebot',
                                           'joint_values':
                                           'place1_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:890 y:658
            OperatableStateMachine.add(
                'Move R1 to place',
                MoveitToJointsDynState(move_group=pick1_group,
                                       offset=0.55,
                                       tool_link=gripper1,
                                       action_topic='/move_group'),
                transitions={
                    'reached': 'finished',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'place1_configuration',
                    'joint_names': 'joint_names'
                })

            # x:882 y:494
            OperatableStateMachine.add(
                'LocateTurtlebot',
                LocateFactoryDeviceState(model_name='mobile_base',
                                         output_frame_id='world'),
                transitions={
                    'succeeded': 'Compute place Turtlebot',
                    'failed': 'failed'
                },
                autonomy={
                    'succeeded': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'pose_turtlebot'})

            # x:915 y:398
            OperatableStateMachine.add('Move R1 back Home',
                                       SrdfStateToMoveit(
                                           config_name='R1Home',
                                           move_group=pick1_group,
                                           action_topic='/move_group',
                                           robot_name=''),
                                       transitions={
                                           'reached': 'Move R1 Home',
                                           'planning_failed': 'failed',
                                           'control_failed': 'failed',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name',
                                           'move_group': 'move_group',
                                           'robot_name': 'robot_name',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:56 y:468
            OperatableStateMachine.add('Move R2 back Home',
                                       SrdfStateToMoveit(
                                           config_name='R2Home',
                                           move_group=pick2_group,
                                           action_topic='/move_group',
                                           robot_name=''),
                                       transitions={
                                           'reached': 'finished',
                                           'planning_failed': 'failed',
                                           'control_failed': 'failed',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name',
                                           'move_group': 'move_group',
                                           'robot_name': 'robot_name',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1011 y:56
            OperatableStateMachine.add(
                'Locate Part Camera1',
                DetectPartCameraState(ref_frame='robot1_base',
                                      camera_topic='/hrwros/logical_camera_1',
                                      camera_frame='logical_camera_1_frame'),
                transitions={
                    'continue': 'Compute Pick for R1',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'part_pose'})

            # x:508 y:144
            OperatableStateMachine.add('Wait for part',
                                       SubscriberState(
                                           topic='/break_beam_sensor_change',
                                           blocking=True,
                                           clear=False),
                                       transitions={
                                           'received': 'Stop feeder',
                                           'unavailable': 'failed'
                                       },
                                       autonomy={
                                           'received': Autonomy.Off,
                                           'unavailable': Autonomy.Off
                                       },
                                       remapping={'message': 'message'})

            # x:58 y:55
            OperatableStateMachine.add('Move R1 Home',
                                       SrdfStateToMoveit(
                                           config_name='R1Home',
                                           move_group=pick1_group,
                                           action_topic='/move_group',
                                           robot_name=''),
                                       transitions={
                                           'reached': 'Start Conveyor',
                                           'planning_failed': 'failed',
                                           'control_failed': 'failed',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name',
                                           'move_group': 'move_group',
                                           'robot_name': 'robot_name',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:212 y:22
            OperatableStateMachine.add('Start Conveyor',
                                       SetConveyorPowerState(stop=False),
                                       transitions={
                                           'succeeded': 'Start feeder',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'speed': 'speed'})

            # x:817 y:25
            OperatableStateMachine.add('Stop Conveyor',
                                       SetConveyorPowerState(stop=True),
                                       transitions={
                                           'succeeded': 'Locate Part Camera1',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'speed': 'speed'})

        return _state_machine
    def create(self):
        names1 = [
            'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
            'robot1_elbow_joint', 'robot1_wrist_1_joint',
            'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
        ]
        pick1_group = 'robot1'
        robot1_loc = Pose2D(x=3.8, y=2.2, theta=-90.0)
        gripper1 = "vacuum_gripper1_suction_cup"
        robot2_loc = Pose2D(x=-4.3, y=-0.9, theta=0.0)
        pick2_group = 'robot2'
        gripper2 = "vacuum_gripper2_suction_cup"
        names2 = [
            'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
            'robot2_elbow_joint', 'robot2_wrist_1_joint',
            'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
        ]
        # x:31 y:297, x:742 y:423
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'])
        _state_machine.userdata.part_pose = []
        _state_machine.userdata.robot1_loc = robot1_loc
        _state_machine.userdata.pose_turtlebot = []
        _state_machine.userdata.pick1_configuration = []
        _state_machine.userdata.place1_configuration = []
        _state_machine.userdata.conveyor_speed = 100
        _state_machine.userdata.robot2_loc = robot2_loc
        _state_machine.userdata.pick2_configuration = []
        _state_machine.userdata.place2_configuration = []

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:13 y:113
            OperatableStateMachine.add(
                'Move R1 Home',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R1Home',
                    move_group=pick1_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Navigate to robot1',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1347 y:308
            OperatableStateMachine.add('Compute pick',
                                       ComputeGraspState(group=pick1_group,
                                                         offset=0.0,
                                                         joint_names=names1,
                                                         tool_link=gripper1,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue': 'Activate Gripper R1',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'part_pose',
                                           'joint_values':
                                           'pick1_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:1169 y:9
            OperatableStateMachine.add('Start feeder',
                                       ControlFeederState(activation=True),
                                       transitions={
                                           'succeeded': 'Wait for part',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:1342 y:99
            OperatableStateMachine.add('Stop feeder',
                                       ControlFeederState(activation=False),
                                       transitions={
                                           'succeeded': 'stop conveyor',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:1336 y:382
            OperatableStateMachine.add('Activate Gripper R1',
                                       VacuumGripperControlState(
                                           enable=True,
                                           service_name='/gripper1/control'),
                                       transitions={
                                           'continue': 'Move R1 to pick',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:561 y:24
            OperatableStateMachine.add('Compute place Turtlebot',
                                       ComputeGraspState(group=pick1_group,
                                                         offset=0.8,
                                                         joint_names=names1,
                                                         tool_link=gripper1,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue': 'Move R1 to place',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'pose_turtlebot',
                                           'joint_values':
                                           'place1_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:379 y:32
            OperatableStateMachine.add(
                'LocateTurtlebot',
                LocateFactoryDeviceState(model_name='mobile_base',
                                         output_frame_id='world'),
                transitions={
                    'succeeded': 'Compute place Turtlebot',
                    'failed': 'failed'
                },
                autonomy={
                    'succeeded': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'pose_turtlebot'})

            # x:1323 y:841
            OperatableStateMachine.add(
                'Move R1 back Home',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R1Home',
                    move_group=pick1_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Move R2 Home',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1339 y:240
            OperatableStateMachine.add(
                'Detect Part Camera',
                DetectPartCameraState(ref_frame='robot1_base',
                                      camera_topic='/hrwros/logical_camera_1',
                                      camera_frame='logical_camera_1_frame'),
                transitions={
                    'continue': 'Compute pick',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'part_pose'})

            # x:1369 y:26
            OperatableStateMachine.add('Wait for part',
                                       SubscriberState(
                                           topic='/break_beam_sensor_change',
                                           blocking=True,
                                           clear=True),
                                       transitions={
                                           'received': 'Stop feeder',
                                           'unavailable': 'failed'
                                       },
                                       autonomy={
                                           'received': Autonomy.Off,
                                           'unavailable': Autonomy.Off
                                       },
                                       remapping={'message': 'message'})

            # x:742 y:11
            OperatableStateMachine.add(
                'Move R1 to place',
                hrwros_factory_states__MoveitToJointsDynState(
                    move_group=pick1_group,
                    offset=0.0,
                    tool_link=gripper1,
                    action_topic='/move_group'),
                transitions={
                    'reached': 'start conveyor',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'place1_configuration',
                    'joint_names': 'joint_names'
                })

            # x:948 y:8
            OperatableStateMachine.add('start conveyor',
                                       SetConveyorPowerState(stop=False),
                                       transitions={
                                           'succeeded': 'Start feeder',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'speed': 'conveyor_speed'})

            # x:1338 y:171
            OperatableStateMachine.add('stop conveyor',
                                       SetConveyorPowerState(stop=True),
                                       transitions={
                                           'succeeded': 'Detect Part Camera',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'succeeded': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'speed': 'conveyor_speed'})

            # x:205 y:59
            OperatableStateMachine.add('Navigate to robot1',
                                       hrwros_factory_states__MoveBaseState(),
                                       transitions={
                                           'arrived': 'LocateTurtlebot',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'arrived': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'waypoint': 'robot1_loc'})

            # x:1313 y:762
            OperatableStateMachine.add('Deactivate gripper 1',
                                       VacuumGripperControlState(
                                           enable=False,
                                           service_name='/gripper1/control'),
                                       transitions={
                                           'continue': 'Move R1 back Home',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:1351 y:448
            OperatableStateMachine.add(
                'Move R1 to pick',
                hrwros_factory_states__MoveitToJointsDynState(
                    move_group=pick1_group,
                    offset=0.0,
                    tool_link=gripper1,
                    action_topic='/move_group'),
                transitions={
                    'reached': 'Move R1 PreGrasp',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'pick1_configuration',
                    'joint_names': 'joint_names'
                })

            # x:1007 y:842
            OperatableStateMachine.add('navigate to robot2',
                                       hrwros_factory_states__MoveBaseState(),
                                       transitions={
                                           'arrived': 'Detect Part Camera_R2',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'arrived': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'waypoint': 'robot2_loc'})

            # x:839 y:839
            OperatableStateMachine.add(
                'Detect Part Camera_R2',
                DetectPartCameraState(ref_frame='robot2_base',
                                      camera_topic='/hrwros/logical_camera_2',
                                      camera_frame='logical_camera_2_frame'),
                transitions={
                    'continue': 'Compute pick_R2',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'part_pose'})

            # x:685 y:836
            OperatableStateMachine.add('Compute pick_R2',
                                       ComputeGraspState(group=pick2_group,
                                                         offset=0.0,
                                                         joint_names=names2,
                                                         tool_link=gripper2,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue': 'Activate Gripper R2',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'part_pose',
                                           'joint_values':
                                           'pick2_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:307 y:840
            OperatableStateMachine.add(
                'Move R2 to pick',
                hrwros_factory_states__MoveitToJointsDynState(
                    move_group=pick2_group,
                    offset=0.0,
                    tool_link=gripper2,
                    action_topic='/move_group'),
                transitions={
                    'reached': 'Move R2 Home_2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'pick2_configuration',
                    'joint_names': 'joint_names'
                })

            # x:487 y:839
            OperatableStateMachine.add('Activate Gripper R2',
                                       VacuumGripperControlState(
                                           enable=True,
                                           service_name='/gripper2/control'),
                                       transitions={
                                           'continue': 'Move R2 to pick',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

            # x:1344 y:566
            OperatableStateMachine.add(
                'Move R1 Home_2',
                hrwros_factory_states__SrdfStateToMoveit(
                    config_name='R1Home',
                    move_group=pick1_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Compute place Turtlebot_2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1323 y:696
            OperatableStateMachine.add(
                'Move R1 to place_2',
                hrwros_factory_states__MoveitToJointsDynState(
                    move_group=pick1_group,
                    offset=0.0,
                    tool_link=gripper1,
                    action_topic='/move_group'),
                transitions={
                    'reached': 'Deactivate gripper 1',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'place1_configuration',
                    'joint_names': 'joint_names'
                })

            # x:1307 y:631
            OperatableStateMachine.add('Compute place Turtlebot_2',
                                       ComputeGraspState(group=pick1_group,
                                                         offset=0.6,
                                                         joint_names=names1,
                                                         tool_link=gripper1,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue': 'Move R1 to place_2',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'pose_turtlebot',
                                           'joint_values':
                                           'place1_configuration',
                                           'joint_names': 'joint_names'
                                       })

            # x:1342 y:505
            OperatableStateMachine.add(
                'Move R1 PreGrasp',
                hrwros_factory_states__SrdfStateToMoveit(
                    config_name='R1PreGrasp',
                    move_group=pick1_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Move R1 Home_2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:95 y:817
            OperatableStateMachine.add(
                'Move R2 Home_2',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R2Home',
                    move_group=pick2_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Move R2Place',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:30 y:721
            OperatableStateMachine.add(
                'Move R2Place',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R2Place',
                    move_group=pick2_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'Deactivate Gripper R2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1150 y:847
            OperatableStateMachine.add(
                'Move R2 Home',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R2Home',
                    move_group=pick2_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'navigate to robot2',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:34 y:486
            OperatableStateMachine.add(
                'Move R2 back Home',
                flexbe_manipulation_states__SrdfStateToMoveit(
                    config_name='R2Home',
                    move_group=pick2_group,
                    action_topic='/move_group',
                    robot_name=''),
                transitions={
                    'reached': 'finished',
                    'planning_failed': 'failed',
                    'control_failed': 'failed',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name',
                    'move_group': 'move_group',
                    'robot_name': 'robot_name',
                    'action_topic': 'action_topic',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:22 y:588
            OperatableStateMachine.add('Deactivate Gripper R2',
                                       VacuumGripperControlState(
                                           enable=False,
                                           service_name='/gripper2/control'),
                                       transitions={
                                           'continue': 'Move R2 back Home',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       })

        return _state_machine
Exemple #6
0
    def create(self):
        pick_group = 'robot1'
        home1 = [1.57, -1.57, 1.24, -1.57, -1.57, 0]
        names1 = [
            'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
            'robot1_elbow_joint', 'robot1_wrist_1_joint',
            'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
        ]
        gripper1 = "vacuum_gripper1_suction_cup"
        # x:1080 y:384, x:225 y:414
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'])
        _state_machine.userdata.part_pose = []
        _state_machine.userdata.pick_configuration = []
        _state_machine.userdata.home1 = home1
        _state_machine.userdata.conveyor_speed = 100

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:189 y:44
            OperatableStateMachine.add(
                'Detect Part',
                DetectPartCameraState(ref_frame='robot1_base',
                                      camera_topic='/hrwros/logical_camera_1',
                                      camera_frame='logical_camera_1_frame'),
                transitions={
                    'continue': 'ComputePoseConf:',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'part_pose'})

            # x:623 y:170
            OperatableStateMachine.add(
                'Move Robot1 to pick',
                flexbe_manipulation_states__MoveitToJointsDynState(
                    move_group=pick_group, action_topic='/move_group'),
                transitions={
                    'reached': 'ActivateGripper',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'pick_configuration',
                    'joint_names': 'joint_names'
                })

            # x:659 y:263
            OperatableStateMachine.add(
                'ActivateGripper',
                VacuumGripperControlState(enable=True,
                                          service_name='/gripper1/control'),
                transitions={
                    'continue': 'Move Robot to home configuration',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                })

            # x:628 y:362
            OperatableStateMachine.add(
                'Move Robot to home configuration',
                flexbe_manipulation_states__MoveitToJointsDynState(
                    move_group=pick_group, action_topic='/move_group'),
                transitions={
                    'reached': 'finished',
                    'planning_failed': 'failed',
                    'control_failed': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off
                },
                remapping={
                    'joint_values': 'home1',
                    'joint_names': 'joint_names'
                })

            # x:598 y:77
            OperatableStateMachine.add('ComputePoseConf:',
                                       ComputeGraspState(group=pick_group,
                                                         offset=0.0,
                                                         joint_names=names1,
                                                         tool_link=gripper1,
                                                         rotation=3.1415),
                                       transitions={
                                           'continue': 'Move Robot1 to pick',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'pose': 'part_pose',
                                           'joint_values':
                                           'pick_configuration',
                                           'joint_names': 'joint_names'
                                       })

        return _state_machine