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
