def pick(self, x, y):
self.moveTo(x, y, pickparam['hight'])
ioOutput0 = SetOutputIO()
ioOutput0.ioNumber = 1
ioOutput0.status = SetOutputIO.IO_HIGH
self.ioPub.publish(ioOutput0)
rospy.sleep(1)
ioOutput1 = SetOutputIO()
ioOutput1.ioNumber = 2
ioOutput1.status = SetOutputIO.IO_HIGH
self.ioPub.publish(ioOutput1)
rospy.sleep(1)
def place(self, x, y):
self.moveTo(x, y, placeparam['placehight'])
rospy.sleep(0.3)
ioOutput0 = SetOutputIO()
ioOutput0.ioNumber = 1
ioOutput0.status = SetOutputIO.IO_LOW
self.ioPub.publish(ioOutput0)
rospy.sleep(1)
ioOutput1 = SetOutputIO()
ioOutput1.ioNumber = 2
ioOutput1.status = SetOutputIO.IO_LOW
self.ioPub.publish(ioOutput1)
rospy.sleep(1)
def pick(self, x, y):
self.moveTo(x, y, 0.21)
ioOutput0 = SetOutputIO()
ioOutput0.ioNumber = 1
ioOutput0.status = SetOutputIO.IO_HIGH
self.ioPub.publish(ioOutput0)
rospy.sleep(1)
def place(self, x, y):
self.ro(1.57, 5)
pose = self.arm.get_current_pose(self.end_effector_link)
print pose
self.moveTo2(x, y, 0.27, pose.pose.orientation.x,
pose.pose.orientation.y, pose.pose.orientation.z,
pose.pose.orientation.w)
ioOutput0 = SetOutputIO()
ioOutput0.ioNumber = 1
ioOutput0.status = SetOutputIO.IO_LOW
self.ioPub.publish(ioOutput0)
rospy.sleep(1)