def positionCheck(data, pub):
objects = ['apple', 'cup', 'scissors', 'teddy_bear', 'clock']
if data.obj in objects:
msg = Issue()
msg.issue = "positioning"
msg.object = data.obj
pub.publish(msg)
def dropCheck(data, pub):
robot_pos = data.robots[0].pose.position
goal_pos = data.robots[0].goal.position
holding = data.robots[0].holding
if prevholding[0] and not holding and robot_pos != goal_pos:
msg = Issue()
msg.issue = "dropped"
msg.object = prevholding[0]
pub.publish(msg)
rospy.loginfo(msg.issue)
prevholding[0] = holding
def positionCheck(data, pub):
objects=['apple','cup','scissors', 'teddy_bear', 'clock','sandwich']
if data.obj in objects:
pub_undo = rospy.Publisher('/' + prevholding[0] + '_dropped', String, queue_size=10)
print prevholding[0] + '_dropped'
undo_msg = "obj_posCheck"
rospy.sleep(1)
pub_undo.publish(undo_msg)
msg = Issue()
msg.issue = "positioning"
msg.object= data.obj
pub.publish(msg)
def dropCheck(data, pub):
robot_pos=data.robots[0].pose.position
goal_pos=data.robots[0].goal.position
holding=data.robots[0].holding
if prevholding[0] and not holding and robot_pos!=goal_pos:
pub_undo = rospy.Publisher('/' + prevholding[0] + '_dropped', String, queue_size=10)
print prevholding[0] + '_dropped'
undo_msg = "obj_dropped"
rospy.sleep(1)
pub_undo.publish(undo_msg)
msg= Issue()
msg.issue = "dropped"
msg.object = prevholding[0]
pub.publish(msg)
rospy.loginfo(msg.issue)
prevholding[0]= holding
def dropCheck(data, pub):
robot_pos = data.robots[0].pose.position
goal_pos = data.robots[0].goal.position
holding = data.robots[0].holding
if prevholding[0] and not holding and robot_pos != goal_pos:
print "OBJECT HAS BEEN DROPPED"
# calls stop service to stop robot since obj was dropped
rospy.wait_for_service("stop")
try:
stop = rospy.ServiceProxy("stop", StopRobot)
resp = stop() # input some params into stop
if (not resp):
raise Exception("stop failure, robot has not stopped")
except rospy.ServiceException, e:
print "Service call failed: %s" % e
msg = Issue()
msg.issue = "dropped"
msg.object = prevholding[0]
pub.publish(msg)
rospy.loginfo(msg.issue)
def visionCheck(data, pub):
msg = Issue()
msg.object = data.object
msg.robot_id = data.robot_id
if data.idx < 0:
msg.issue = "missing"
pub.publish(msg)
#grasp check
else:
robot_x = 0.0
robot_y = -0.45
if data.robot_id != 0:
robot_y = 0.45
xdist = data.pose.position.x - robot_x
ydist = data.pose.position.y - robot_y
dist = math.hypot(xdist, ydist)
if dist > 0.68:
msg.issue = "ungraspable"
pub.publish(msg)
def visionCheck(data, pub):
msg= Issue()
msg.object = data.object
msg.robot_id= data.robot_id
if data.idx < 0:
msg.issue = "missing"
pub.publish(msg)
#grasp check
else:
robot_x = 0.0
robot_y = -0.45
if data.robot_id != 0:
robot_y= 0.45
xdist=data.pose.position.x - robot_x
ydist=data.pose.position.y - robot_y
dist= math.hypot(xdist,ydist)
if dist > 0.68:
pub_undo = rospy.Publisher('/' + prevholding[0] + '_dropped', String, queue_size=10)
print prevholding[0] + '_dropped'
undo_msg = "obj_ungrasp"
rospy.sleep(1)
pub_undo.publish(undo_msg)
msg.issue = "ungraspable"
pub.publish(msg)