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")