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 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):
rospy.loginfo('Executing state PlanCleanUp')
header = Header()
header.frame_id = '/odom_combined'
plan = []
target_zones = []
if (userdata.yaml.task_type == Task.TASK_5):
utils.map.remove_puzzle_fixture(yaml=userdata.yaml)
for puzzle_part in userdata.yaml.relative_puzzle_part_target_poses:
target_position = mathemagie.add_point(
userdata.yaml.puzzle_fixture.position,
puzzle_part.pose.position)
fake_target_zone = TargetZone(name=puzzle_part.name +
"_target",
expected_object=puzzle_part.name,
target_position=target_position,
max_distance=0.05)
# setting a z value here is useless, will be overwritten later...
#fake_target_zone.target_position.z = fake_target_zone.target_position.z + 0.5 # put it above the fixture to avoid collision
print("fake_target_zone = " + str(fake_target_zone))
target_zones.append(fake_target_zone)
else:
target_zones = userdata.yaml.target_zones
tzs_for = {tz.expected_object: tz for tz in target_zones}
found_objects = userdata.objects_found
print("tzs_for = " + str(tzs_for))
# Get the objects that are in an others objects target zone
objects_in_tzs = []
for obj in found_objects:
tz = utils.in_target_zone(obj, userdata.yaml)
if not tz is None and tz.expected_object != obj.mpe_object.id:
objects_in_tzs.append((obj, tz))
# Sort the list so the blue handle will be placed last if possible
# sorted(found_objects, key=lambda obj: len(obj.mpe_object.primitives))
found_objects.sort(key=lambda obj: len(obj.mpe_object.primitives))
# sorted(objects_in_tzs, key=lambda obj_in_tz: len(obj_in_tz[0].mpe_object.primitives))
objects_in_tzs.sort(
key=lambda obj_in_tz: len(obj_in_tz[0].mpe_object.primitives))
# If it's task 5 start with the biggest objects to get more coverage
if userdata.yaml.task_type == Task.TASK_5:
found_objects.reverse()
objects_in_tzs.reverse()
def get_pose(obj):
return PointStamped(header,
tzs_for[obj.mpe_object.id].target_position)
rospy.logdebug('Objects in target zones: %s' % str(objects_in_tzs))
rospy.logdebug('Target zones for objects: %s' % str(tzs_for))
# If no object is in a target zone
if not objects_in_tzs:
plan = map(lambda obj: (obj, get_pose(obj)), found_objects)
# If one object is in a target zone
elif len(objects_in_tzs) == 1:
found_objects.remove(objects_in_tzs[0][0])
plan.append((objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
plan += map(lambda obj: (obj, get_pose(obj)), found_objects)
# If two objects are in a target zone
elif len(objects_in_tzs) == 2:
found_objects.remove(objects_in_tzs[0][0])
found_objects.remove(objects_in_tzs[1][0])
# If the one object is in the target zone of the other object in a target zone
if objects_in_tzs[0][1].name == tzs_for[objects_in_tzs[1]
[0].mpe_object.id].name:
# If the objects in target zone are in the target zone of the other object
if objects_in_tzs[1][1].name == tzs_for[
objects_in_tzs[0][0].mpe_object.id].name:
plan.append(
(objects_in_tzs[1][0],
self._pose_near_target_zone(objects_in_tzs[1][1],
header)))
plan.append(
(objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
plan.append(
(objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan += map(lambda obj: (obj, get_pose(obj)),
found_objects)
else:
plan.append(
(objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
plan.append(
(objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan += map(lambda obj: (obj, get_pose(obj)),
found_objects)
else:
plan.append(
(objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan.append(
(objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
plan += map(lambda obj: (obj, get_pose(obj)), found_objects)
# If all three objects are in a target zone
elif len(objects_in_tzs) == 3:
# Place the first object next to its targetzone
plan.append((objects_in_tzs[0][0],
self._pose_near_target_zone(objects_in_tzs[0][1],
header)))
if objects_in_tzs[1][0].mpe_object.id == objects_in_tzs[0][
1].expected_object:
plan.append(
(objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan.append(
(objects_in_tzs[2][0], get_pose(objects_in_tzs[2][0])))
else:
plan.append(
(objects_in_tzs[2][0], get_pose(objects_in_tzs[2][0])))
plan.append(
(objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan.append((objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
userdata.clean_up_plan = plan
return 'success'
def execute(self, userdata):
rospy.loginfo('Executing state PlanCleanUp')
header = Header()
header.frame_id = '/odom_combined'
plan = []
target_zones = []
if (userdata.yaml.task_type == Task.TASK_5):
utils.map.remove_puzzle_fixture(yaml=userdata.yaml)
for puzzle_part in userdata.yaml.relative_puzzle_part_target_poses:
target_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, puzzle_part.pose.position)
fake_target_zone = TargetZone(name = puzzle_part.name + "_target", expected_object = puzzle_part.name, target_position = target_position, max_distance = 0.05)
# setting a z value here is useless, will be overwritten later...
#fake_target_zone.target_position.z = fake_target_zone.target_position.z + 0.5 # put it above the fixture to avoid collision
print("fake_target_zone = " + str(fake_target_zone))
target_zones.append(fake_target_zone)
else:
target_zones = userdata.yaml.target_zones
tzs_for = {tz.expected_object: tz for tz in target_zones}
found_objects = userdata.objects_found
print ("tzs_for = " + str(tzs_for))
# Get the objects that are in an others objects target zone
objects_in_tzs = []
for obj in found_objects:
tz = utils.in_target_zone(obj, userdata.yaml)
if not tz is None and tz.expected_object != obj.mpe_object.id:
objects_in_tzs.append((obj, tz))
# Sort the list so the blue handle will be placed last if possible
# sorted(found_objects, key=lambda obj: len(obj.mpe_object.primitives))
found_objects.sort(key=lambda obj: len(obj.mpe_object.primitives))
# sorted(objects_in_tzs, key=lambda obj_in_tz: len(obj_in_tz[0].mpe_object.primitives))
objects_in_tzs.sort(key=lambda obj_in_tz: len(obj_in_tz[0].mpe_object.primitives))
# If it's task 5 start with the biggest objects to get more coverage
if userdata.yaml.task_type == Task.TASK_5:
found_objects.reverse()
objects_in_tzs.reverse()
def get_pose(obj):
return PointStamped(header, tzs_for[obj.mpe_object.id].target_position)
rospy.logdebug('Objects in target zones: %s' % str(objects_in_tzs))
rospy.logdebug('Target zones for objects: %s' % str(tzs_for))
# If no object is in a target zone
if not objects_in_tzs:
plan = map(lambda obj: (obj, get_pose(obj)), found_objects)
# If one object is in a target zone
elif len(objects_in_tzs) == 1:
found_objects.remove(objects_in_tzs[0][0])
plan.append((objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
plan += map(lambda obj: (obj, get_pose(obj)), found_objects)
# If two objects are in a target zone
elif len(objects_in_tzs) == 2:
found_objects.remove(objects_in_tzs[0][0])
found_objects.remove(objects_in_tzs[1][0])
# If the one object is in the target zone of the other object in a target zone
if objects_in_tzs[0][1].name == tzs_for[objects_in_tzs[1][0].mpe_object.id].name:
# If the objects in target zone are in the target zone of the other object
if objects_in_tzs[1][1].name == tzs_for[objects_in_tzs[0][0].mpe_object.id].name:
plan.append((objects_in_tzs[1][0],
self._pose_near_target_zone(objects_in_tzs[1][1], header)))
plan.append((objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
plan.append((objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan += map(lambda obj: (obj, get_pose(obj)), found_objects)
else:
plan.append((objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
plan.append((objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan += map(lambda obj: (obj, get_pose(obj)), found_objects)
else:
plan.append((objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan.append((objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
plan += map(lambda obj: (obj, get_pose(obj)), found_objects)
# If all three objects are in a target zone
elif len(objects_in_tzs) == 3:
# Place the first object next to its targetzone
plan.append((objects_in_tzs[0][0],
self._pose_near_target_zone(objects_in_tzs[0][1], header)))
if objects_in_tzs[1][0].mpe_object.id == objects_in_tzs[0][1].expected_object:
plan.append((objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan.append((objects_in_tzs[2][0], get_pose(objects_in_tzs[2][0])))
else:
plan.append((objects_in_tzs[2][0], get_pose(objects_in_tzs[2][0])))
plan.append((objects_in_tzs[1][0], get_pose(objects_in_tzs[1][0])))
plan.append((objects_in_tzs[0][0], get_pose(objects_in_tzs[0][0])))
userdata.clean_up_plan = plan
return 'success'
