class AdjutsState(smach.State): def __init__(self, namespaces): smach.State.__init__(self, outcomes=["adjusted"]) self.__switcher = ClientDistributor( namespaces, ServiceConfig("controller_manager/switch_controller", SwitchController)) self.__enable = False self.__timeout = 0.1 def execute(self, userdata): req = SwitchControllerRequest() req.stop_controllers = ["position_joint_controller"] req.start_controllers = ["cartesian_controller"] req.strictness = 2 self.__switcher.call(req) srv = rospy.Service("~trigger", Empty, self.__callback__) while not self.__enable: rospy.sleep(self.__timeout) self.__enable = False srv.shutdown() req = SwitchControllerRequest() req.stop_controllers = ["cartesian_controller"] req.start_controllers = [] req.strictness = 2 self.__switcher.call(req) return "adjusted" def __callback__(self, req): self.__enable = True return
def __init__(self, namespaces): smach.State.__init__(self, outcomes=["adjusted"]) self.__switcher = ClientDistributor( namespaces, ServiceConfig("controller_manager/switch_controller", SwitchController)) self.__enable = False self.__timeout = 0.1
class FormationControlServiceState(smach.State): def __init__(self, namespaces, name): smach.State.__init__(self, outcomes=["called"]) self.clients = ClientDistributor(namespaces, ServiceConfig(name, Empty)) def execute(self, userdata): self.clients.call(EmptyRequest()) return "called"
class DrivePoseState(smach.State): def __init__(self, namespaces, pose_name): smach.State.__init__(self, outcomes=["done"]) self.__pose_name = pose_name self.__client = ClientDistributor( namespaces, ServiceConfig("move_teached/drive_to", SetName)) self.__sleep_time = rospy.get_param("~drive_sleeptime") def execute(self, userdata): req = SetNameRequest() req.name = self.__pose_name self.__client.call(req) rospy.sleep(self.__sleep_time) return "done"
def __init__(self,base_namespaces,arm_namespaces): smach.State.__init__(self, outcomes=['startup_done',"references_error"], io_keys=['slaves'] ) self.__enable_manipulator=rospy.get_param("~enable_manipulator",False) self.__arm_controller=self.__switcher=ClientDistributor(arm_namespaces,ServiceConfig("controller_manager/switch_controller",SwitchController)) self.__arm_namespaces=arm_namespaces self.__base_namespaces=base_namespaces self.__arm_request=SwitchControllerRequest() self.__arm_request.start_controllers=["position_joint_controller"] self.__arm_request.strictness=2 if self.__enable_manipulator: self.__gripper_clients=ClientDistributor(arm_namespaces,ServiceConfig("grip",SetBool)) self.__formation_disabler=ClientDistributor(base_namespaces,ServiceConfig("slave_controller/disable_controller",Empty))
class StartState(smach.State): def __init__(self,base_namespaces,arm_namespaces): smach.State.__init__(self, outcomes=['startup_done',"references_error"], io_keys=['slaves'] ) self.__enable_manipulator=rospy.get_param("~enable_manipulator",False) self.__arm_controller=self.__switcher=ClientDistributor(arm_namespaces,ServiceConfig("controller_manager/switch_controller",SwitchController)) self.__arm_namespaces=arm_namespaces self.__base_namespaces=base_namespaces self.__arm_request=SwitchControllerRequest() self.__arm_request.start_controllers=["position_joint_controller"] self.__arm_request.strictness=2 if self.__enable_manipulator: self.__gripper_clients=ClientDistributor(arm_namespaces,ServiceConfig("grip",SetBool)) self.__formation_disabler=ClientDistributor(base_namespaces,ServiceConfig("slave_controller/disable_controller",Empty)) def execute(self,userdata): userdata.slaves=dict() for base in self.__base_namespaces: userdata.slaves[base]=dict() #Load reference vectors as poses try: ref=rospy.get_param(base+"/slave_controller/reference") ref_pose=PoseStamped() ref_pose.pose.position.x=ref[0] ref_pose.pose.position.y=ref[1] ref_pose.pose.position.z=0.0 quat=transformations.quaternion_from_euler(0.0,0.0,ref[2]) ref_pose.pose.orientation.x=quat[0] ref_pose.pose.orientation.y=quat[1] ref_pose.pose.orientation.z=quat[2] ref_pose.pose.orientation.w=quat[3] userdata.slaves[base]["reference"]=ref_pose except: return "references_error" if self.__enable_manipulator: req=SetBoolRequest() req.data=False self.__gripper_clients.call(req) self.__formation_disabler.call(EmptyRequest()) self.__arm_controller.call(self.__arm_request) return "startup_done"
class ReleaseObjectState(smach.State): def __init__(self, namespaces): smach.State.__init__(self, outcomes=["released"]) self.__clients = ClientDistributor(namespaces, ServiceConfig("grip", SetBool)) self.__switcher = ClientDistributor( namespaces, ServiceConfig("controller_manager/switch_controller", SwitchController)) self.__enable_orientation_ff = rospy.get_param( "~enable_orientation_ff", False) if self.__enable_orientation_ff: self.__orientation_init = ClientDistributor( namespaces, ServiceConfig("orientation_ff/disable", Empty)) def execute(self, userdata): grip_req = SetBoolRequest() grip_req.data = False self.__clients.call(grip_req) rospy.sleep(2) req = SwitchControllerRequest() req.stop_controllers = ["compliance_controller"] req.start_controllers = ["position_joint_controller"] req.strictness = 2 self.__switcher.call(req) if self.__enable_orientation_ff: self.__orientation_init.call(EmptyRequest()) return "released"
def __init__(self, namespaces): smach.State.__init__(self, outcomes=["released"]) self.__clients = ClientDistributor(namespaces, ServiceConfig("grip", SetBool)) self.__switcher = ClientDistributor( namespaces, ServiceConfig("controller_manager/switch_controller", SwitchController)) self.__enable_orientation_ff = rospy.get_param( "~enable_orientation_ff", False) if self.__enable_orientation_ff: self.__orientation_init = ClientDistributor( namespaces, ServiceConfig("orientation_ff/disable", Empty))
def __init__(self, namespaces, name): smach.State.__init__(self, outcomes=["called"]) self.clients = ClientDistributor(namespaces, ServiceConfig(name, Empty))
def __init__(self, namespaces, pose_name): smach.State.__init__(self, outcomes=["done"]) self.__pose_name = pose_name self.__client = ClientDistributor( namespaces, ServiceConfig("move_teached/drive_to", SetName)) self.__sleep_time = rospy.get_param("~drive_sleeptime")