diff --git a/pyrobosim/pyrobosim/core/locations.py b/pyrobosim/pyrobosim/core/locations.py index 19ff3d95..720d60de 100644 --- a/pyrobosim/pyrobosim/core/locations.py +++ b/pyrobosim/pyrobosim/core/locations.py @@ -139,14 +139,15 @@ def set_pose(self, pose): if "offset" in self.metadata["footprint"]: p_off = self.metadata["footprint"]["offset"] else: - p_off = (0, 0) + p_off = (0.0, 0.0) for p in self.metadata["nav_poses"]: - rot_p = rot2d((p[0] + p_off[0], p[1] + p_off[1]), self.pose.get_yaw()) + p = Pose.construct(p) + rot_p = rot2d((p.x + p_off[0], p.y + p_off[1]), self.pose.get_yaw()) nav_pose = Pose( x=rot_p[0] + self.pose.x, y=rot_p[1] + self.pose.y, z=self.pose.z, - yaw=p[2] + self.pose.get_yaw(), + yaw=p.get_yaw() + self.pose.get_yaw(), ) if self.parent.is_collision_free(nav_pose): self.nav_poses.append(nav_pose) @@ -285,16 +286,17 @@ def set_pose_from_parent(self): if "offset" in self.metadata["footprint"]: p_off = self.metadata["footprint"]["offset"] else: - p_off = (0, 0) + p_off = (0.0, 0.0) for p in self.metadata["nav_poses"]: + p = Pose.construct(p) rot_p = rot2d( - (p[0] + p_off[0], p[1] + p_off[1]), self.parent.pose.get_yaw() + (p.x + p_off[0], p.y + p_off[1]), self.parent.pose.get_yaw() ) nav_pose = Pose( x=rot_p[0] + self.parent.pose.x, y=rot_p[1] + self.parent.pose.y, z=self.parent.pose.z, - yaw=p[2] + self.parent.pose.get_yaw(), + yaw=p.get_yaw() + self.parent.pose.get_yaw(), ) if self.parent.parent.is_collision_free(nav_pose): self.nav_poses.append(nav_pose) diff --git a/pyrobosim/pyrobosim/core/yaml_utils.py b/pyrobosim/pyrobosim/core/yaml_utils.py index 5e8eb571..33538de9 100644 --- a/pyrobosim/pyrobosim/core/yaml_utils.py +++ b/pyrobosim/pyrobosim/core/yaml_utils.py @@ -96,7 +96,8 @@ def add_locations(self): """Add locations for object spawning to the world.""" for loc_data in self.data.get("locations", []): loc_args = copy.deepcopy(loc_data) - loc_args["pose"] = Pose.construct(loc_args["pose"]) + if "pose" in loc_args: + loc_args["pose"] = Pose.construct(loc_args["pose"]) self.world.add_location(**loc_args) def add_objects(self): diff --git a/pyrobosim/pyrobosim/data/example_location_data.yaml b/pyrobosim/pyrobosim/data/example_location_data.yaml index 0947902e..f900c3b9 100644 --- a/pyrobosim/pyrobosim/data/example_location_data.yaml +++ b/pyrobosim/pyrobosim/data/example_location_data.yaml @@ -8,8 +8,14 @@ table: dims: [0.9, 1.2] height: 0.5 nav_poses: - - [-0.75, 0, 0] - - [0.75, 0, 3.14] + - position: # left + x: -0.75 + y: 0.0 + - position: # right + x: 0.75 + y: 0.0 + rotation_eul: + yaw: 3.14 locations: - name: "tabletop" footprint: @@ -31,10 +37,24 @@ desk: footprint: type: parent nav_poses: - - [0, -0.5, 1.57] - - [-0.5, 0, 0] - - [0, 0.5, -1.57] - - [0.5, 0, 3.14] + - position: # below + x: 0.0 + y: -0.5 + rotation_eul: + yaw: 1.57 + - position: # left + x: -0.5 + y: 0.0 + - position: # above + x: 0.0 + y: 0.5 + rotation_eul: + yaw: -1.57 + - position: # right + x: 0.5 + y: 0.0 + rotation_eul: + yaw: 3.14 counter: footprint: @@ -52,8 +72,16 @@ counter: - [-0.25, 0.25] offset: [0.3, 0] nav_poses: - - [0, 0.5, -1.57] - - [0, -0.5, 1.57] + - position: # below + x: 0.0 + y: -0.5 + rotation_eul: + yaw: 1.57 + - position: # above + x: 0.0 + y: 0.5 + rotation_eul: + yaw: -1.57 - name: "right" footprint: type: polygon @@ -64,8 +92,16 @@ counter: - [-0.25, 0.25] offset: [-0.3, 0] nav_poses: - - [0, 0.5, -1.57] - - [0, -0.5, 1.57] + - position: # below + x: 0.0 + y: -0.5 + rotation_eul: + yaw: 1.57 + - position: # above + x: 0.0 + y: 0.5 + rotation_eul: + yaw: -1.57 color: [0, 0.2, 0] trash_can: @@ -79,8 +115,14 @@ trash_can: type: parent padding: 0.05 nav_poses: - - [0.5, 0.0, 3.14] - - [-0.5, 0.0, 0.0] + - position: # left + x: -0.5 + y: 0.0 + - position: # right + x: 0.5 + y: 0.0 + rotation_eul: + yaw: 3.14 color: [0, 0.35, 0.2] charger: @@ -97,8 +139,22 @@ charger: footprint: type: parent nav_poses: - - [0, -0.35, 1.57] - - [-0.5, 0, 0] - - [0, 0.35, -1.57] - - [0.5, 0, 3.14] + - position: # below + x: 0.0 + y: -0.5 + rotation_eul: + yaw: 1.57 + - position: # left + x: -0.35 + y: 0.0 + - position: # above + x: 0.0 + y: 0.5 + rotation_eul: + yaw: -1.57 + - position: # right + x: 0.35 + y: 0.0 + rotation_eul: + yaw: 3.14 color: [0.4, 0.4, 0]