def add_yaml(self, yaml):
"""
Adds some fixed collision objects that are defined in the yaml description
:param yaml: yaml description of the task
:type: Task
"""
self.add_ground()
if yaml is None:
self.add_cam_mast(0.92, 0.92)
else:
x = yaml.mast_of_cam.base_pose.linear.x
y = yaml.mast_of_cam.base_pose.linear.y
self.add_cam_mast(x, y)
if len(yaml.relative_puzzle_part_target_poses) == 0:
return
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
pose.pose = deepcopy(yaml.puzzle_fixture)
#TODO: WTF?
#if you change the Point shit here, also change it in map.py!
# pose.pose.position = add_point(pose.pose.position, Point(0.115 if yaml.puzzle_fixture.position.x < 0 else -0.18, 0.165 if yaml.puzzle_fixture.position.y < 0 else 0.05, 0))
pose.pose.position = get_puzzle_fixture_center(yaml.puzzle_fixture)
box = self.make_box("puzzle", pose, [0.32, 0.32, 0.1])
print("puzzle_fixture collision object = " + str(box))
self.safe_objects.append(box.id)
self.add_object(box)
def add_yaml(self, yaml):
"""
Adds some fixed collision objects that are defined in the yaml description
:param yaml: yaml description of the task
:type: Task
"""
self.add_ground()
if yaml is None:
self.add_cam_mast(0.92, 0.92)
else:
x = yaml.mast_of_cam.base_pose.linear.x
y = yaml.mast_of_cam.base_pose.linear.y
self.add_cam_mast(x, y)
if len(yaml.relative_puzzle_part_target_poses) == 0:
return
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
pose.pose = deepcopy(yaml.puzzle_fixture)
#TODO: WTF?
#if you change the Point shit here, also change it in map.py!
# pose.pose.position = add_point(pose.pose.position, Point(0.115 if yaml.puzzle_fixture.position.x < 0 else -0.18, 0.165 if yaml.puzzle_fixture.position.y < 0 else 0.05, 0))
pose.pose.position = get_puzzle_fixture_center(yaml.puzzle_fixture)
box = self.make_box("puzzle", pose, [0.32, 0.32, 0.1])
print("puzzle_fixture collision object = "+str(box))
self.safe_objects.append(box.id)
self.add_object(box)
def odomCallback(odomData):
global lastOdom,tfl,lastMapPose
lastOdom = odomData
temp = PoseStampedMsg()
temp.pose = lastOdom.pose.pose
temp.header = lastOdom.header
try:
#now lastmappose has map coords
tfl.transformPose("map",temp,lastMapPose)
except tf.TransformException:
rospy.roserror("Transform Error")
