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")