Exemple #1
0
import tf

from controller import Controller
from geometry_msgs.msg import PoseStamped
from dynamixel_workbench_msgs.srv import SetPID
from object_detection.srv import ObjectDetection

from geometry_msgs.msg import Pose, Point, Quaternion

from gp import GaussianProcess

# Initialize controller
ctrl = Controller()
# Initialize GP
gp = GaussianProcess()
gp.fit_GP()
MIN_JOINT_MOTION_FOR_HAND_OVER = 0.1


# define state Idle
class Idle(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['gotToolInput'])

    def execute(self, userdata):
        ctrl.open_gripper()
        # ctrl.set_camera_angles(ctrl.HOME_POS_CAMERA_01)
        ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_00)
        rospy.loginfo('Executing state IDLE')

        # Return success