def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Last input update
self._last_update = rospy.get_time()
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Retrieve the publish rate
self._publish_rate = 1000
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._command_sub = rospy.Subscriber('gripper_control/command',
EndeffectorCommand,
self._on_gripper_command)
self._command = 'stop'
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = self._gripper.get_position_ratio(
self._gripper.closed_pos)
print 'ratio=', self._ratio_goal
self._kp = 100
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._controller_update_timer = rospy.Timer(
rospy.Duration(1.0 / self._publish_rate), self._update)
rospy.on_shutdown(self._on_shutdown)
def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Initial values for the gripper joystick buttons
# Default - B button from XBox 360 controller
self._open_button = 1
if rospy.has_param('~open_button'):
self._open_button = rospy.get_param('~open_button')
# Default - X button from XBox 360 controller
self._close_button = 2
if rospy.has_param('~close_button'):
self._close_button = rospy.get_param('~close_button')
# Default - RB button from XBox 360 controller
self._deadman_button = 5
if rospy.has_param('~deadman_button'):
self._deadman_button = rospy.get_param('~deadman_button')
if rospy.has_param('~exclusion_buttons'):
self._exclusion_buttons = rospy.get_param('~exclusion_buttons')
if type(self._exclusion_buttons) in [float, int]:
self._exclusion_buttons = [int(self._exclusion_buttons)]
elif type(self._exclusion_buttons) == list:
for n in self._exclusion_buttons:
if type(n) != float:
raise rospy.ROSException(
'Exclusion buttons must be an'
' integer index to the joystick button')
else:
self._exclusion_buttons = list()
self._joy_gain = 0.1
if rospy.has_param('~joy_gain'):
self._joy_gain = rospy.get_param('~joy_gain')
# Retrieve the publish rate
self._publish_rate = 100
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = 0.0
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
else:
self._kp = 100.0
if rospy.has_param('~ki'):
self._ki = rospy.get_param('~ki')
else:
self._ki = 0.0
if rospy.has_param('~kd'):
self._kd = rospy.get_param('~kd')
else:
self._kd = 0.0
self._controllers = dict()
self._controllers[self._gripper.control_joint] = PIDRegulator(
self._kp, self._ki, self._kd, 100)
self._controllers[self._gripper.mimic_joint] = PIDRegulator(
self._kp, self._ki, self._kd, 100)
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback)
rate = rospy.Rate(self._publish_rate)
while not rospy.is_shutdown():
self._update()
rate.sleep()