def _update_gripper_stat(self, position):
stat = GripperStat()
stat.header.stamp = rospy.get_rostime()
stat.header.seq = self._seq
stat.is_ready = self._driver_ready
stat.is_reset = False
stat.is_moving = False
stat.obj_detected = False
stat.fault_status = self._driver_state
stat.position = position
stat.requested_position = position
stat.current = 0
self._seq += 1
return stat
def _update_gripper_stat(self, dev=0):
stat = GripperStat()
stat.header.stamp = rospy.get_rostime()
stat.header.seq = self._seq[dev]
stat.is_ready = self._gripper.is_ready(dev)
stat.is_reset = self._gripper.is_reset(dev)
stat.is_moving = self._gripper.is_moving(dev)
stat.obj_detected = self._gripper.object_detected(dev)
stat.fault_status = self._gripper.get_fault_status(dev)
stat.position = self._gripper.get_pos(dev)
stat.requested_position = self._gripper.get_req_pos(dev)
stat.current = self._gripper.get_current(dev)
self._seq[dev] += 1
return stat
def _update_gripper_stat(self,dev=0):
stat = GripperStat()
stat.header.stamp = rospy.get_rostime()
stat.header.seq = self._seq[dev]
stat.is_ready = self._gripper.is_ready(dev)
stat.is_reset = self._gripper.is_reset(dev)
stat.is_moving = self._gripper.is_moving(dev)
stat.obj_detected = self._gripper.object_detected(dev)
stat.fault_status = self._gripper.get_fault_status(dev)
stat.position = self._gripper.get_pos(dev)
stat.requested_position = self._gripper.get_req_pos(dev)
stat.current = self._gripper.get_current(dev)
self._seq[dev]+=1
return stat
def _create_gripper_stat_msg(self, dev):
"""
create a GripperState ROS message based on current status of gripper
"""
stat = GripperStat()
stat.header.stamp = rospy.get_rostime()
stat.header.seq = self._seq[dev]
stat.is_ready = self._gripper.is_ready(dev)
stat.is_reset = self._gripper.is_reset(dev)
stat.is_moving = self._gripper.is_moving(dev)
stat.obj_detected = self._gripper.object_detected(dev)
stat.fault_status = self._gripper.get_fault_status(dev)
stat.position = self._gripper.get_pos(dev)
stat.requested_position = self._gripper.get_req_pos(dev)
stat.current = self._gripper.get_current(dev)
self._seq[dev] += 1
return stat
def _run_driver(self):
count = 0
last_time = rospy.get_time()
r = rospy.Rate(10)
while not rospy.is_shutdown():
dt = rospy.get_time() - last_time
if (0 == self._driver_state):
print "In state 0 - Deactivate"
for i in range(self._num_grippers):
if (dt < 0.5 and count < 4):
count += 1
self._gripper.deactivate_gripper(i)
else:
self._driver_state = 1
elif (1 == self._driver_state):
print "In state 1 - Activating"
grippers_activated = True
for i in range(self._num_grippers):
self._gripper.activate_gripper(i)
grippers_activated &= self._gripper.is_ready(i)
if (grippers_activated):
self._driver_state = 2
rospy.sleep(5)
elif (2 == self._driver_state):
print "In state 2 - Ready"
self._driver_ready = True
for i in range(self._num_grippers):
success = True
print "Starting: {}".format(success)
success &= self._gripper.process_act_cmd(i)
print "After ACT: {}".format(success)
success &= self._gripper.process_stat_cmd(i)
print "After STAT: {}".format(success)
if not success:
rospy.logerr("Failed to contact gripper %d"%i)
else:
stat = GripperStat()
js = JointState()
stat = self._update_gripper_stat(i)
js = self._update_gripper_joint_state(i)
if (1 == self._num_grippers):
self._gripper_pub.publish(stat)
self._gripper_joint_state_pub.publish(js)
else:
if (i == 0):
self._left_gripper_pub.publish(stat)
self._left_gripper_joint_state_pub.publish(js)
else:
self._right_gripper_pub.publish(stat)
self._right_gripper_joint_state_pub.publish(js)
r.sleep()
self._gripper.shutdown()
def __init__(self):
super(UR5, self).__init__()
# Instantiate a `RobotCommander`_ object. Provides information such as the robot's
# kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
# Instantiate a `MoveGroupCommander`_ object. This object is an interface
# to a planning group (group of joints). In this tutorial the group is the primary
# arm joints in the Panda robot, so we set the group's name to "panda_arm".
# If you are using a different robot, change this value to the name of your robot
# arm planning group.
# This interface can be used to plan and execute motions:
arm_name = "manipulator"
arm = moveit_commander.MoveGroupCommander(arm_name)
gripper_name = "gripper"
gripper = moveit_commander.MoveGroupCommander(gripper_name)
# Create a `DisplayTrajectory`_ ROS publisher which is used to display
# trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# Settings about the gripper
rospy.Subscriber("/gripper/stat", GripperStat, self._update_gripper_stat, queue_size=10)
self._gripper_pub = rospy.Publisher('/gripper/cmd', GripperCmd, queue_size=10)
self._gripper_stat = GripperStat()
self._gripper_cmd = GripperCmd()
# We can also print the name of the end-effector link for this group:
eef_link = arm.get_end_effector_link()
print("============ End effector link for arm: %s" % eef_link)
eef_link = gripper.get_end_effector_link()
print("============ End effector link for gripper: %s" % eef_link)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print("============ Available Planning Groups:", robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(robot.get_current_state())
print("")
self.robot = robot
self.arm = arm
self.gripper = gripper
self.display_trajectory_publisher = display_trajectory_publisher
self.group_names = group_names
self.init_pose = self.arm.get_current_pose().pose
self.waiting = True
self.object_poses = {}
self.tfListener = tf.TransformListener()
def __init__(self,loop_rate,grip_threshold,speed):
self._loop_delay = 1 / loop_rate
self._grip_threshold = grip_threshold
self._speed = speed
self._statSub = rospy.Subscriber('/gripper/stat', GripperStat, self._update_gripper_stat, queue_size=10)
self._cmdPub = rospy.Publisher('/gripper/cmd', GripperCmd, queue_size=10)
self._stat = GripperStat()
self._action_server = actionlib.SimpleActionServer('gripper_command', GripperCommandAction, self._execute, False)
self._action_server.start()
def __init__(self):
self._num_grippers = rospy.get_param('~num_grippers', 1)
self.open = rospy.get_param('~open', True)
self.close = rospy.get_param('~close', False)
self._prefix = rospy.get_param('~prefix', None)
if (self._num_grippers == 1 and self._prefix is None):
rospy.Subscriber("/gripper/stat",
GripperStat,
self._update_gripper_stat,
queue_size=10)
self._gripper_pub = rospy.Publisher('/gripper/cmd',
GripperCmd,
queue_size=10)
elif (self._num_grippers == 1 and self._prefix is not None):
rospy.logwarn('gripper prefix = {}'.format(self._prefix))
rospy.Subscriber("/" + self._prefix + "gripper/stat",
GripperStat,
self._update_gripper_stat,
queue_size=10)
self._gripper_pub = rospy.Publisher('/' + self._prefix +
'gripper/cmd',
GripperCmd,
queue_size=10)
elif (self._num_grippers == 2):
rospy.Subscriber("/left_gripper/stat",
GripperStat,
self._update_gripper_stat,
queue_size=10)
self._left_gripper_pub = rospy.Publisher('/left_gripper/stat',
GripperCmd,
queue_size=10)
rospy.Subscriber("/right_gripper/stat",
GripperStat,
self._update_right_gripper_stat,
queue_size=10)
self._right_gripper_pub = rospy.Publisher('/right_gripper/cmd',
GripperCmd,
queue_size=10)
else:
rospy.logerr(
"Number of grippers not supported (needs to be 1 or 2)")
return
self._gripper_stat = [GripperStat()] * self._num_grippers
self._gripper_cmd = [GripperCmd()] * self._num_grippers
self._run_test()
def __init__(self):
self._statSub = rospy.Subscriber('/gripper/stat',
GripperStat,
self._update_gripper_stat,
queue_size=10)
self._cmdPub = rospy.Publisher('/gripper/cmd',
GripperCmd,
queue_size=10)
self._stat = GripperStat()
self._action_server = actionlib.SimpleActionServer(
'gripper/gripper_command', GripperCommandAction, self._execute,
False)
self._action_server.start()
def _run_driver(self):
last_time = rospy.get_time()
r = rospy.Rate(100)
while not rospy.is_shutdown():
dt = rospy.get_time() - last_time
if (0 == self._driver_state):
for i in range(self._num_grippers):
if (dt < 0.5):
self._gripper.deactivate_gripper(i)
else:
self._driver_state = 1
elif (1 == self._driver_state):
grippers_activated = True
for i in range(self._num_grippers):
self._gripper.activate_gripper(i)
grippers_activated &= self._gripper.is_ready(i)
if (grippers_activated):
self._driver_state = 2
elif (2 == self._driver_state):
self._driver_ready = True
for i in range(self._num_grippers):
success = True
success &= self._gripper.process_act_cmd(i)
success &= self._gripper.process_stat_cmd(i)
if not success:
rospy.logerr("Failed to contact gripper %d" % i)
else:
stat = GripperStat()
js = JointState()
stat = self._update_gripper_stat(i)
js = self._update_gripper_joint_state(i)
if (1 == self._num_grippers):
self._gripper_pub.publish(stat)
self._gripper_joint_state_pub.publish(js)
else:
if (i == 0):
self._left_gripper_pub.publish(stat)
self._left_gripper_joint_state_pub.publish(js)
else:
self._right_gripper_pub.publish(stat)
self._right_gripper_joint_state_pub.publish(js)
r.sleep()
self._gripper.shutdown()
def __init__(self):
self._num_grippers = rospy.get_param('~num_grippers',1)
if (self._num_grippers == 1):
rospy.Subscriber("/gripper/stat", GripperStat, self._update_gripper_stat, queue_size=10)
self._gripper_pub = rospy.Publisher('/gripper/cmd', GripperCmd, queue_size=10)
elif (self._num_grippers == 2):
rospy.Subscriber("/left_gripper/stat", GripperStat, self._update_gripper_stat, queue_size=10)
self._left_gripper_pub = rospy.Publisher('/left_gripper/stat', GripperCmd, queue_size=10)
rospy.Subscriber("/right_gripper/stat", GripperStat, self._update_right_gripper_stat, queue_size=10)
self._right_gripper_pub = rospy.Publisher('/right_gripper/cmd', GripperCmd, queue_size=10)
else:
rospy.logerr("Number of grippers not supported (needs to be 1 or 2)")
return
self._gripper_stat = [GripperStat()] * self._num_grippers
self._gripper_cmd = [GripperCmd()] * self._num_grippers
self._run_test()
def __init__(self):
self.num_of_grippers = rospy.get_param("~num_grippers", 1)
# make sure 1 gripper is detected
if self.num_of_grippers != 1:
LOG.ERROR("Incorrect number of grippers detected: {}".format(
self.num_of_grippers))
return
else:
LOG.INFO("Correct number of gripper detected.")
rospy.Subscriber("/gripper/stat",
GripperStat,
self.update_gripper_stat,
queue_size=10)
self.gripper_publisher = rospy.Publisher('/gripper/cmd',
GripperCmd,
queue_size=10)
self.gripper_stat = GripperStat()
self.gripper_cmd = GripperCmd()
# set the gripper force and speed to default value
self.set_gripper_force(100.0)
self.set_gripper_speed(0.02)
self.r = rospy.Rate(1)