def _tighten_hand(self, pos_increment=0.8):
self._disable_tactile_stops()
pos_cmd = PoseCommand()
pos_cmd.f1 = self.hand_state.finger[0].proximal + pos_increment
pos_cmd.f2 = self.hand_state.finger[1].proximal + pos_increment
pos_cmd.f3 = self.hand_state.finger[2].proximal + pos_increment
pos_cmd.preshape = 0.0
for i in range(10):
self.pos_cmd_pub.publish(pos_cmd)
def open_hand(self, req):
pos_cmd = PoseCommand()
pos_cmd.f1 = 0.0
pos_cmd.f2 = 0.0
pos_cmd.f3 = 0.0
pos_cmd.preshape = 0.0
for i in range(10):
self.pos_cmd_pub.publish(pos_cmd)
self.grasp_status = False
return TriggerResponse(success=True)
import sys, rospy, cv2
from reflex_msgs.msg import PoseCommand
from cibr_img_processing.msg import Ints
global cmd, is_grasping
is_grasping = False
cmd = PoseCommand()
cmd.f1 = 0.0
cmd.f2 = 0.0
cmd.f3 = 0.0
def vission_cb(data):
global cmd
cmd.preshape = (0.0 if data[0] else 2.0)
if not is_grasping: command_pub.publish(cmd)
def handy():
global cmd
if not is_grasping:
cmd.f1 = 0.8
cmd.f2 = 0.8
cmd.f3 = 0.8
command_pub.publish(cmd)
is_grasping = True
print("Grasping")
else:
cmd.f1 = 0.0
cmd.f2 = 0.0
cmd.f3 = 0.0
