def in_target_zone(euroc_object, yaml):
if (yaml.task_type == Task.TASK_5):
for puzzle_part in yaml.relative_puzzle_part_target_poses:
centroid = deepcopy(euroc_object.c_centroid)
centroid.z = 0
target_position = mathemagie.add_point(
yaml.puzzle_fixture.position, puzzle_part.pose.position)
fake_target_zone = TargetZone
fake_target_zone.name = puzzle_part.name + "_target"
fake_target_zone.expected_object = puzzle_part.name
fake_target_zone.target_position = target_position
fake_target_zone.max_distance = 0.05 # dafuq do i know?
dist = mathemagie.euclidean_distance(centroid, target_position)
if dist < fake_target_zone.max_distance:
return fake_target_zone
else:
for target_zone in yaml.target_zones:
centroid = deepcopy(euroc_object.c_centroid)
centroid.z = 0
dist = mathemagie.euclidean_distance(centroid,
target_zone.target_position)
if dist < target_zone.max_distance:
return target_zone
return None
def _handle_focus_object(self, req):
# Read the objects_to_focus if this is the first iteration
taskdata = req.taskdata
taskdata.objects_to_focus = taskdata.classified_objects
if self._objects_to_focus is None:
self._objects_to_focus = taskdata.objects_to_focus
print("Objects to focus1: ")
print(self._objects_to_focus)
if not taskdata.focused_point is None:
object_to_focus = None
min_dist = 100
for obj in self._objects_to_focus:
dist = mathemagie.euclidean_distance(
obj.c_centroid, taskdata.focused_point)
if dist < min_dist:
min_dist = dist
object_to_focus = obj
self._objects_to_focus = [object_to_focus]
# Remember the fitted objects
elif not taskdata.fitted_object is None:
self._fitted_objects.append(taskdata.fitted_object)
if not utils.map is None:
position = taskdata.fitted_object.mpe_object.primitive_poses[
0].position
utils.map.mark_region_as_object_under_point(
position.x, position.y)
rospy.logdebug(str(utils.map))
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
# If there are no more objects to focus
if not self._objects_to_focus:
self._objects_to_focus = None
print("Objects to focus is None 2")
taskdata.fitted_objects = self._fitted_objects
self._fitted_objects = []
return TaskDataServiceResponse(taskdata=taskdata, result="success")
# Set the next object to focus
next_object = self._objects_to_focus.pop()
print("Objects to focus2")
print(self._objects_to_focus)
taskdata.object_to_focus = next_object
print("nect_object:")
print(next_object)
if utils.is_handle(next_object.object.id, taskdata.yaml):
return TaskDataServiceResponse(taskdata=taskdata,
result="focusHandle")
else:
return TaskDataServiceResponse(taskdata=taskdata,
result="focusObject")
def _handle_focus_object(self, req):
# Read the objects_to_focus if this is the first iteration
taskdata = req.taskdata
taskdata.objects_to_focus = taskdata.classified_objects
if self._objects_to_focus is None:
self._objects_to_focus = taskdata.objects_to_focus
print("Objects to focus1: ")
print(self._objects_to_focus)
if not taskdata.focused_point is None:
object_to_focus = None
min_dist = 100
for obj in self._objects_to_focus:
dist = mathemagie.euclidean_distance(obj.c_centroid, taskdata.focused_point)
if dist < min_dist:
min_dist = dist
object_to_focus = obj
self._objects_to_focus = [object_to_focus]
# Remember the fitted objects
elif not taskdata.fitted_object is None:
self._fitted_objects.append(taskdata.fitted_object)
if not utils.map is None:
position = taskdata.fitted_object.mpe_object.primitive_poses[0].position
utils.map.mark_region_as_object_under_point(position.x, position.y)
rospy.logdebug(str(utils.map))
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
# If there are no more objects to focus
if not self._objects_to_focus:
self._objects_to_focus = None
print("Objects to focus is None 2")
taskdata.fitted_objects = self._fitted_objects
self._fitted_objects = []
return TaskDataServiceResponse(taskdata = taskdata, result = "success")
# Set the next object to focus
next_object = self._objects_to_focus.pop()
print("Objects to focus2")
print(self._objects_to_focus)
taskdata.object_to_focus = next_object
print("nect_object:")
print(next_object)
if utils.is_handle(next_object.object.id, taskdata.yaml):
return TaskDataServiceResponse(taskdata = taskdata, result = "focusHandle")
else:
return TaskDataServiceResponse(taskdata = taskdata, result = "focusObject")
def in_target_zone(euroc_object, yaml):
if (yaml.task_type == Task.TASK_5):
for puzzle_part in yaml.relative_puzzle_part_target_poses:
centroid = deepcopy(euroc_object.c_centroid)
centroid.z = 0
target_position = mathemagie.add_point(yaml.puzzle_fixture.position, puzzle_part.pose.position)
fake_target_zone = TargetZone
fake_target_zone.name = puzzle_part.name + "_target"
fake_target_zone.expected_object = puzzle_part.name
fake_target_zone.target_position = target_position
fake_target_zone.max_distance = 0.05 # dafuq do i know?
dist = mathemagie.euclidean_distance(centroid, target_position)
if dist < fake_target_zone.max_distance:
return fake_target_zone
else:
for target_zone in yaml.target_zones:
centroid = deepcopy(euroc_object.c_centroid)
centroid.z = 0
dist = mathemagie.euclidean_distance(centroid, target_zone.target_position)
if dist < target_zone.max_distance:
return target_zone
return None
def execute(self, userdata):
# Read the objects_to_focus if this is the first iteration
if self._objects_to_focus is None:
self._objects_to_focus = userdata.objects_to_focus
if not userdata.focused_point is None:
object_to_focus = None
min_dist = 100
for obj in self._objects_to_focus:
dist = mathemagie.euclidean_distance(obj.c_centroid, userdata.focused_point)
if dist < min_dist:
min_dist = dist
object_to_focus = obj
self._objects_to_focus = [object_to_focus]
# Remember the fitted objects
elif not userdata.fitted_object is None:
self._fitted_objects.append(userdata.fitted_object)
if not utils.map is None:
position = userdata.fitted_object.mpe_object.primitive_poses[0].position
utils.map.mark_region_as_object_under_point(position.x, position.y)
rospy.logdebug(str(utils.map))
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
# If there are no more objects to focus
if not self._objects_to_focus:
self._objects_to_focus = None
userdata.fitted_objects = self._fitted_objects
self._fitted_objects = []
return 'success'
# Set the next object to focus
next_object = self._objects_to_focus.pop()
userdata.object_to_focus = next_object
if utils.is_handle(next_object.object.id, userdata.yaml):
return 'focusHandle'
else:
return 'focusObject'
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 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'