def _handle_focus(self, req):
taskdata = req.taskdata
rospy.loginfo('Executing state FocusObject')
rospy.loginfo('Trying to focus %s' % taskdata.object_to_focus.object.id)
taskdata.focused_object = None
centroid = taskdata.object_to_focus.c_centroid
centroid.z -= 0.03
poses = calc_grasp_position.make_scan_pose(centroid, *utils.focus_poses[self._ctr])
utils.manipulation.set_planning_time_arm(2)
move_method = utils.manipulation.move_to
transition = ""
for pose in poses:
if move_method(pose):
taskdata.focused_object = taskdata.object_to_focus
rospy.logdebug('Wait for clock')
time.sleep(1)
rospy.logdebug('Wait for tf again.')
rospy.sleep(4)
self._ctr = 0
transition = self.RETURN_VAL_SUCSS
if transition == "":
utils.manipulation.set_planning_time_arm(5)
if self._ctr < 6:
self._ctr += 1
transition = self.RETURN_VAL_RETRY
else:
self._ctr = 0
transition = self.RETURN_VAL_FAIL
return TaskDataServiceResponse(taskdata = taskdata, result = transition)
def _handle_focus(self, req):
taskdata = req.taskdata
rospy.loginfo('Executing state FocusObject')
rospy.loginfo('Trying to focus %s' %
taskdata.object_to_focus.object.id)
taskdata.focused_object = None
centroid = taskdata.object_to_focus.c_centroid
centroid.z -= 0.03
poses = calc_grasp_position.make_scan_pose(
centroid, *utils.focus_poses[self._ctr])
utils.manipulation.set_planning_time_arm(2)
move_method = utils.manipulation.move_to
transition = ""
for pose in poses:
if move_method(pose):
taskdata.focused_object = taskdata.object_to_focus
rospy.logdebug('Wait for clock')
time.sleep(1)
rospy.logdebug('Wait for tf again.')
rospy.sleep(4)
self._ctr = 0
transition = self.RETURN_VAL_SUCSS
if transition == "":
utils.manipulation.set_planning_time_arm(5)
if self._ctr < 6:
self._ctr += 1
transition = self.RETURN_VAL_RETRY
else:
self._ctr = 0
transition = self.RETURN_VAL_FAIL
return TaskDataServiceResponse(taskdata=taskdata, result=transition)
def test11_1(self):
m = self.make_free_map()
m.get_cell_by_index(9,9).set_unknown()
m.get_cell_by_index(9,10).set_unknown()
m.get_cell_by_index(9,11).set_unknown()
m.get_cell_by_index(10,9).set_unknown()
m.get_cell_by_index(10,10).set_unknown()
m.get_cell_by_index(10,11).set_unknown()
m.get_cell_by_index(11,9).set_unknown()
m.get_cell_by_index(11,10).set_unknown()
m.get_cell_by_index(11,11).set_unknown()
(x, y) = m.index_to_coordinates(10, 10)
poses = make_scan_pose(Point(x,y,0), 0.05, 0, n=16)
poses = m.filter_invalid_scan_poses(x, y, poses)
# print len(poses)
self.assertTrue(len(poses) == 0, poses)
def test11_1(self):
m = self.make_free_map()
m.get_cell_by_index(9, 9).set_unknown()
m.get_cell_by_index(9, 10).set_unknown()
m.get_cell_by_index(9, 11).set_unknown()
m.get_cell_by_index(10, 9).set_unknown()
m.get_cell_by_index(10, 10).set_unknown()
m.get_cell_by_index(10, 11).set_unknown()
m.get_cell_by_index(11, 9).set_unknown()
m.get_cell_by_index(11, 10).set_unknown()
m.get_cell_by_index(11, 11).set_unknown()
(x, y) = m.index_to_coordinates(10, 10)
poses = make_scan_pose(Point(x, y, 0), 0.05, 0, n=16)
poses = m.filter_invalid_scan_poses(x, y, poses)
# print len(poses)
self.assertTrue(len(poses) == 0, poses)
def test12_1(self):
m = self.make_free_map()
# m.get_cell_by_index(9,9).set_unknown()
# m.get_cell_by_index(9,10).set_unknown()
# m.get_cell_by_index(9,11).set_unknown()
m.get_cell_by_index(10,9).set_obstacle()
m.get_cell_by_index(10,9).highest_z = 1
m.get_cell_by_index(10,10).set_unknown()
m.get_cell_by_index(10,11).set_obstacle()
m.get_cell_by_index(10,11).highest_z = 1
# m.get_cell_by_index(11,9).set_unknown()
# m.get_cell_by_index(11,10).set_unknown()
# m.get_cell_by_index(11,11).set_unknown()
(x, y) = m.index_to_coordinates(10, 10)
poses = make_scan_pose(Point(x,y,0), 0.15, 0, n=16)
# print ""
# print len(poses)
poses = m.filter_invalid_scan_poses2(x, y, poses)
# print len(poses)
self.assertTrue(len(poses) == 10, len(poses))
def test12_1(self):
m = self.make_free_map()
# m.get_cell_by_index(9,9).set_unknown()
# m.get_cell_by_index(9,10).set_unknown()
# m.get_cell_by_index(9,11).set_unknown()
m.get_cell_by_index(10, 9).set_obstacle()
m.get_cell_by_index(10, 9).highest_z = 1
m.get_cell_by_index(10, 10).set_unknown()
m.get_cell_by_index(10, 11).set_obstacle()
m.get_cell_by_index(10, 11).highest_z = 1
# m.get_cell_by_index(11,9).set_unknown()
# m.get_cell_by_index(11,10).set_unknown()
# m.get_cell_by_index(11,11).set_unknown()
(x, y) = m.index_to_coordinates(10, 10)
poses = make_scan_pose(Point(x, y, 0), 0.15, 0, n=16)
# print ""
# print len(poses)
poses = m.filter_invalid_scan_poses2(x, y, poses)
# print len(poses)
self.assertTrue(len(poses) == 10, len(poses))
def execute(self, userdata):
rospy.loginfo('Executing state FocusObject')
rospy.loginfo('Trying to focus %s' % userdata.object_to_focus.object.id)
userdata.focused_object = None
centroid = userdata.object_to_focus.c_centroid
centroid.z -= 0.03
poses = calc_grasp_position.make_scan_pose(centroid, *utils.focus_poses[self._ctr])
if userdata.enable_movement:
move_method = utils.manipulation.move_arm_and_base_to
else:
utils.manipulation.set_planning_time_arm(2)
move_method = utils.manipulation.move_to
for pose in poses:
if move_method(pose):
userdata.focused_object = userdata.object_to_focus
rospy.logdebug('Wait for clock')
time.sleep(1)
rospy.logdebug('Wait for tf again.')
rospy.sleep(4)
self._ctr = 0
return 'success'
utils.manipulation.set_planning_time_arm(5)
if self._ctr < 6:
self._ctr += 1
return 'retry'
else:
self._ctr = 0
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state ScanObstacles')
userdata.sec_try_done = False
if userdata.sec_try:
current_region = self.classified_regions[self.next_cluster - 1][0]
userdata.sec_try_done = True
else:
#get regions
if len(self.classified_regions) == 0:
obstacle_cluster = utils.map.get_obstacle_regions()
rospy.logdebug(
str(len(self.classified_regions)) + " regions found.")
print '#####################################'
self.classified_regions = utils.map.undercover_classifier(
obstacle_cluster, userdata.yaml.objects)
print self.classified_regions
print '#####################################'
base = utils.manipulation.get_base_origin().point
self.classified_regions.sort(key=lambda x: euclidean_distance(
Point(*(utils.map.index_to_coordinates(*x[0].get_avg())) +
(0.0, )), base))
# x[0].get_number_of_cells())
if self.next_cluster >= len(self.classified_regions):
rospy.loginfo("searched all cluster")
return 'noRegionLeft'
current_region = self.classified_regions[self.next_cluster][0]
rospy.logdebug("current region: " + str(self.next_cluster) + "\n" +
str(current_region) + "\nclassified as " +
str(self.classified_regions[self.next_cluster][1]))
self.next_cluster += 1
region_centroid = Point(
*(utils.map.index_to_coordinates(*current_region.get_avg())) +
(-0.065, ))
dist_to_region = mathemagie.euclidean_distance(Point(0, 0, 0),
region_centroid)
#if userdata.yaml.task_type == Task.TASK_5:
# fixture_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, Point(0.115, 0.165, 0))
# dist_to_fixture = mathemagie.euclidean_distance(fixture_position, region_centroid)
# if dist_to_fixture < 0.35:
# rospy.logdebug("Region classified as puzzle fixture, skipping")
# return 'mapScanned'
# If the arm cannot move ignore distant regions
# TODO find the best max distance
if not userdata.enable_movement:
rospy.logdebug('Distance of the current region to the arm: %s' %
str(dist_to_region))
if dist_to_region > 1.1:
rospy.logwarn('Current region is out of reach. Ignoring it.')
return 'mapScanned'
angle = 1.2
distance = 0.6 + current_region.get_number_of_cells() * 0.008
poses = make_scan_pose(region_centroid, distance, angle, n=16)
if not userdata.enable_movement:
poses = utils.manipulation.filter_close_poses(poses)
poses = utils.map.filter_invalid_scan_poses2(region_centroid.x,
region_centroid.y, poses)
if userdata.sec_try:
current_pose = utils.manipulation.get_eef_position().pose.position
current_pose.z = 0
region_to_eef = subtract_point(region_centroid, current_pose)
poses.sort(key=lambda pose: abs(
get_angle(
region_to_eef,
subtract_point(
Point(pose.pose.position.x, pose.pose.position.y, 0),
region_centroid)) - pi / 2))
visualize_poses(poses)
if userdata.enable_movement:
# move = utils.manipulation.move_arm_and_base_to
plan = utils.manipulation.plan_arm_and_base_to
else:
# move = utils.manipulation.move_to
plan = utils.manipulation.plan_arm_to
utils.manipulation.blow_up_objects(do_not_blow_up_list=("map"))
for pose in poses:
# utils.manipulation.set_planning_time_arm(2)
if utils.manipulation.move_with_plan_to(plan(pose)):
# utils.manipulation.set_planning_time_arm(5)
userdata.focused_point = region_centroid
rospy.logdebug('Wait for clock')
time.sleep(0.5)
rospy.sleep(2.5)
return 'newImage'
utils.manipulation.blow_down_objects()
return 'mapScanned'
def execute(self, userdata):
rospy.loginfo('Executing state ScanMapArmCam')
if self.finished:
return 'mapScanned'
regions = utils.map.get_unknown_regions()
regions.sort(key=lambda r: -r.get_number_of_cells())
for r in regions:
r.set_boarder_cells(sorted(utils.map.get_boarder_cell_points(r),
cmp=lambda p1, p2: utils.map.is_more_edge(p1.x, p1.y, p2.x, p2.y)))
i = 0
r_id = 0
utils.manipulation.set_planning_time_arm_base(5)
while len(regions) > 0 and utils.map.get_percent_cleared() < 0.985:
if len(regions[r_id].get_boarder_cells()) < self.min_num_of_cells:
regions.remove(regions[r_id])
if len(regions) == 0:
break
r_id = r_id % len(regions)
i = 0
continue
if i >= 2:
r_id += 1
r_id = r_id % len(regions)
i = 0
continue
next_point = regions[r_id].get_boarder_cells()[-1]
regions[r_id].get_boarder_cells().remove(next_point)
cell_x = next_point.x
cell_y = next_point.y
utils.map.mark_cell(cell_x, cell_y, True)
next_point.z = 0.2
poses = make_scan_pose(next_point, 0.7, 0.8, n=16)
poses = utils.map.filter_invalid_scan_poses(cell_x, cell_y, poses)
poses = utils.map.filter_invalid_scan_poses2(cell_x, cell_y, poses)
poses.sort(key=lambda pose: -euclidean_distance(
Point(*(utils.map.index_to_coordinates(*regions[r_id].get_avg()) + (0,))),
Point(pose.pose.position.x, pose.pose.position.y, 0)))
visualize_poses(poses)
j = 0
move_successfull = False
while j < len(poses) and not move_successfull:
rospy.logdebug("try scan pose nr. " + str(j))
move_successfull = utils.manipulation.move_arm_and_base_to(poses[j], do_not_blow_up_list=("all"))
j += 1
if move_successfull:
rospy.sleep(utils.waiting_time_before_scan)
arm_base = utils.manipulation.get_base_origin()
map_updated = utils.map.add_point_cloud(arm_base.point, scene_cam=False)
if not map_updated:
rospy.logdebug("no map update")
utils.map.mark_cell(next_point.x, next_point.y, False)
i += 1
continue
rospy.logdebug("published")
utils.map.mark_cell(next_point.x, next_point.y, False)
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
utils.manipulation.set_planning_time_arm_base(10)
return 'newImage'
utils.map.mark_cell(next_point.x, next_point.y, False)
i += 1
rospy.loginfo("can't update map any further")
utils.map.all_unknowns_to_obstacle()
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
self.finished = True
utils.manipulation.set_planning_time_arm_base(10)
return 'mapScanned'
def execute(self, userdata):
rospy.loginfo('Executing state ScanObstacles')
userdata.sec_try_done = False
if userdata.sec_try:
current_region = self.classified_regions[self.next_cluster-1][0]
userdata.sec_try_done = True
else:
#get regions
if len(self.classified_regions) == 0:
obstacle_cluster = utils.map.get_obstacle_regions()
rospy.logdebug(str(len(self.classified_regions)) + " regions found.")
print '#####################################'
self.classified_regions = utils.map.undercover_classifier(obstacle_cluster, userdata.yaml.objects)
print self.classified_regions
print '#####################################'
base = utils.manipulation.get_base_origin().point
self.classified_regions.sort(key=lambda x: euclidean_distance(Point(*(utils.map.index_to_coordinates(*x[0].get_avg()))+(0.0,)), base))
# x[0].get_number_of_cells())
if self.next_cluster >= len(self.classified_regions):
rospy.loginfo("searched all cluster")
return 'noRegionLeft'
current_region = self.classified_regions[self.next_cluster][0]
rospy.logdebug("current region: " + str(self.next_cluster) + "\n" + str(current_region) +
"\nclassified as " + str(self.classified_regions[self.next_cluster][1]))
self.next_cluster += 1
region_centroid = Point(*(utils.map.index_to_coordinates(*current_region.get_avg()))+(-0.065,))
dist_to_region = mathemagie.euclidean_distance(Point(0, 0, 0), region_centroid)
#if userdata.yaml.task_type == Task.TASK_5:
# fixture_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, Point(0.115, 0.165, 0))
# dist_to_fixture = mathemagie.euclidean_distance(fixture_position, region_centroid)
# if dist_to_fixture < 0.35:
# rospy.logdebug("Region classified as puzzle fixture, skipping")
# return 'mapScanned'
# If the arm cannot move ignore distant regions
# TODO find the best max distance
if not userdata.enable_movement:
rospy.logdebug('Distance of the current region to the arm: %s' % str(dist_to_region))
if dist_to_region > 1.1:
rospy.logwarn('Current region is out of reach. Ignoring it.')
return 'mapScanned'
angle = 1.2
distance = 0.6 + current_region.get_number_of_cells()*0.008
poses = make_scan_pose(region_centroid, distance, angle, n=16)
if not userdata.enable_movement:
poses = utils.manipulation.filter_close_poses(poses)
poses = utils.map.filter_invalid_scan_poses2(region_centroid.x, region_centroid.y, poses)
if userdata.sec_try:
current_pose = utils.manipulation.get_eef_position().pose.position
current_pose.z = 0
region_to_eef = subtract_point(region_centroid, current_pose)
poses.sort(key=lambda pose: abs(get_angle(region_to_eef, subtract_point(Point(pose.pose.position.x, pose.pose.position.y, 0), region_centroid)) - pi/2))
visualize_poses(poses)
if userdata.enable_movement:
# move = utils.manipulation.move_arm_and_base_to
plan = utils.manipulation.plan_arm_and_base_to
else:
# move = utils.manipulation.move_to
plan = utils.manipulation.plan_arm_to
utils.manipulation.blow_up_objects(do_not_blow_up_list=("map"))
for pose in poses:
# utils.manipulation.set_planning_time_arm(2)
if utils.manipulation.move_with_plan_to(plan(pose)):
# utils.manipulation.set_planning_time_arm(5)
userdata.focused_point = region_centroid
rospy.logdebug('Wait for clock')
time.sleep(0.5)
rospy.sleep(2.5)
return 'newImage'
utils.manipulation.blow_down_objects()
return 'mapScanned'
