def id_obj(obj_ids):
pos_arm.set_pose(weigh_obj_pose)
weight = wgh_obj.get_obj_weight()
# filter obj ids by weight
filt_obj_ids = []
for i in range(0, len(obj_ids)):
obj_id = obj_ids[i]
obj_weight = obj_weights[obj_id]
if obj_weight > (weight * 0.8) and obj_weight < (weight * 1.2):
filt_obj_ids.append(obj_id)
rospy.loginfo(obj_id)
if len(filt_obj_ids) == 0:
return ""
pos_arm.set_pose(rec_obj_pose)
return rec_obj.get_obj_id(filt_obj_ids)
def id_obj(obj_ids):
pos_arm.set_pose(weigh_obj_pose);
weight = wgh_obj.get_obj_weight()
# filter obj ids by weight
filt_obj_ids = [ ]
for i in range(0, len(obj_ids)):
obj_id = obj_ids[i]
obj_weight = obj_weights[obj_id]
if obj_weight > (weight * 0.8) and obj_weight < (weight * 1.2):
filt_obj_ids.append(obj_id)
rospy.loginfo(obj_id)
if len(filt_obj_ids) == 0:
return ""
pos_arm.set_pose(rec_obj_pose)
return rec_obj.get_obj_id(filt_obj_ids)
def pick_obj():
pos_arm.set_pose(observe_obj_pose)
obj_pos = dep_obj.get_obj_pos()
temp_observe_obj_pose = geometry_msgs.msg.Pose()
temp_observe_obj_pose.position.x = observe_obj_pose.position.x
temp_observe_obj_pose.position.y = observe_obj_pose.position.y
temp_observe_obj_pose.position.z = observe_obj_pose.position.z
temp_observe_obj_pose.orientation.w = observe_obj_pose.orientation.w
temp_observe_obj_pose.orientation.x = observe_obj_pose.orientation.x
temp_observe_obj_pose.orientation.y = observe_obj_pose.orientation.y
temp_observe_obj_pose.orientation.z = observe_obj_pose.orientation.z
temp_x = 0.0
temp_y = 0.0
temp_angle = 0.0
while obj_pos == "":
# move arm to slightly different position
temp_x = temp_x + 0.05 * numpy.cos(temp_angle) - 1.0
temp_y = temp_y + 0.05 * numpy.sin(temp_angle)
temp_observe_obj_pose.position.x = observe_obj_pose.position.x + temp_x
temp_observe_obj_pose.position.y = observe_obj_pose.position.y + temp_y
temp_angle = temp_angle + 0.52
pos_arm.set_pose(temp_observe_obj_pose)
obj_pos = dep_obj.get_obj_pos()
coords = obj_pos.split(",")
# move arm over object
new_pick_obj_pose = geometry_msgs.msg.Pose()
new_pick_obj_pose.position.x = temp_observe_obj_pose.position.x - 0.055 * (
float(coords[1]) - 215)
new_pick_obj_pose.position.y = temp_observe_obj_pose.position.y - 0.055 * (
float(coords[0]) - 240)
new_pick_obj_pose.position.z = temp_observe_obj_pose.position.z
new_pick_obj_pose.orientation.w = temp_observe_obj_pose.orientation.w
new_pick_obj_pose.orientation.x = temp_observe_obj_pose.orientation.x
new_pick_obj_pose.orientation.y = temp_observe_obj_pose.orientation.y
new_pick_obj_pose.orientation.z = temp_observe_obj_pose.orientation.z
pos_arm.set_pose(new_pick_obj_pose)
while sns_obj.is_obj_sns() == False:
# move arm towards object
new_pick_obj_pose.position.z = new_pick_obj_pose.position.z - 0.01
pos_arm.set_pose(new_pick_obj_pose)
continue
man_obj.grab_obj()
def pick_obj():
pos_arm.set_pose(observe_obj_pose)
obj_pos = dep_obj.get_obj_pos()
temp_observe_obj_pose = geometry_msgs.msg.Pose()
temp_observe_obj_pose.position.x = observe_obj_pose.position.x
temp_observe_obj_pose.position.y = observe_obj_pose.position.y
temp_observe_obj_pose.position.z = observe_obj_pose.position.z
temp_observe_obj_pose.orientation.w = observe_obj_pose.orientation.w
temp_observe_obj_pose.orientation.x = observe_obj_pose.orientation.x
temp_observe_obj_pose.orientation.y = observe_obj_pose.orientation.y
temp_observe_obj_pose.orientation.z = observe_obj_pose.orientation.z
temp_x = 0.0;
temp_y = 0.0;
temp_angle = 0.0;
while obj_pos == "":
# move arm to slightly different position
temp_x = temp_x + 0.05 * numpy.cos(temp_angle) - 1.0
temp_y = temp_y + 0.05 * numpy.sin(temp_angle)
temp_observe_obj_pose.position.x = observe_obj_pose.position.x + temp_x
temp_observe_obj_pose.position.y = observe_obj_pose.position.y + temp_y
temp_angle = temp_angle + 0.52
pos_arm.set_pose(temp_observe_obj_pose)
obj_pos = dep_obj.get_obj_pos()
coords = obj_pos.split(",")
# move arm over object
new_pick_obj_pose = geometry_msgs.msg.Pose()
new_pick_obj_pose.position.x = temp_observe_obj_pose.position.x - 0.055 * ( float(coords[1]) - 215 )
new_pick_obj_pose.position.y = temp_observe_obj_pose.position.y - 0.055 * ( float(coords[0]) - 240 )
new_pick_obj_pose.position.z = temp_observe_obj_pose.position.z
new_pick_obj_pose.orientation.w = temp_observe_obj_pose.orientation.w
new_pick_obj_pose.orientation.x = temp_observe_obj_pose.orientation.x
new_pick_obj_pose.orientation.y = temp_observe_obj_pose.orientation.y
new_pick_obj_pose.orientation.z = temp_observe_obj_pose.orientation.z
pos_arm.set_pose(new_pick_obj_pose)
while sns_obj.is_obj_sns() == False:
# move arm towards object
new_pick_obj_pose.position.z = new_pick_obj_pose.position.z - 0.01
pos_arm.set_pose(new_pick_obj_pose)
continue;
man_obj.grab_obj()
def init():
rospy.init_node("tsk_pln_tst_node")
weights = []
pos_arm.init("pos_arm_srv")
result = pos_arm.set_pose(rec_obj_pose)
for i in range(0, 20):
wgh_obj.init("wgh_obj_srv")
result = wgh_obj.get_obj_weight()
weights.append(result)
rospy.loginfo("Task planner test: " + str(result))
time.sleep(0.5)
rospy.loginfo("Median: " + str(np.median(weights)))
rospy.spin()
def init():
rospy.init_node("tsk_pln_tst_node")
weights = []
pos_arm.init("pos_arm_srv")
result = pos_arm.set_pose(rec_obj_pose)
for i in range(0,20):
wgh_obj.init("wgh_obj_srv")
result = wgh_obj.get_obj_weight()
weights.append(result)
rospy.loginfo("Task planner test: " + str(result))
time.sleep(0.5)
rospy.loginfo("Median: " + str(np.median(weights)))
rospy.spin()
def stow_obj(bin_id):
pos_arm.set_pose(bin_poses[bin_id])
man_obj.drop_obj()
output_file_path = rospy.get_param("output_file_path", "task.json")
data = read_input_file(input_file_path)
tote_obj_ids = get_tote_obj_ids(data)
# stow items in tote
while len(tote_obj_ids) > 0:
pick_obj()
obj_id = id_obj(tote_obj_ids)
rospy.loginfo("object recognised: " + obj_id)
if obj_id == "":
# failed to detect object, put it back
pos_arm.set_pose(pick_obj_pose)
man_obj.drop_obj()
else:
# successful detected object
bin_id = sel_bin(data)
stow_obj(bin_id)
# update data
bin_obj_ids = get_bin_obj_ids(data, bin_id)
bin_obj_ids.append(obj_id)
tote_obj_ids.remove(obj_id)
write_output_file(output_file_path, data)
def stow_obj(bin_id):
pos_arm.set_pose(bin_poses[bin_id])
man_obj.drop_obj()
output_file_path = rospy.get_param("output_file_path", "task.json")
data = read_input_file(input_file_path)
tote_obj_ids = get_tote_obj_ids(data)
# stow items in tote
while len(tote_obj_ids) > 0:
pick_obj()
obj_id = id_obj(tote_obj_ids)
rospy.loginfo("object recognised: " + obj_id)
if obj_id == "":
# failed to detect object, put it back
pos_arm.set_pose(pick_obj_pose)
man_obj.drop_obj()
else:
# successful detected object
bin_id = sel_bin(data)
stow_obj(bin_id)
# update data
bin_obj_ids = get_bin_obj_ids(data, bin_id)
bin_obj_ids.append(obj_id)
tote_obj_ids.remove(obj_id)
write_output_file(output_file_path, data)