(self)
| 595 | return |
| 596 | |
| 597 | np.save(f"{self.tinynav_db_path}/relocalization_poses.npy", self.relocalization_poses, allow_pickle=True) |
| 598 | np.save(f"{self.tinynav_db_path}/relocalization_pose_weights.npy", self.relocalization_pose_weights, allow_pickle=True) |
| 599 | np.save(f"{self.tinynav_db_path}/failed_relocalizations.npy", self.failed_relocalizations, allow_pickle=True) |
| 600 | np.save(f"{self.tinynav_db_path}/poses.npy", self.pose_graph_used_pose, allow_pickle=True) |
| 601 | |
| 602 | logging.info(f"Saved {len(self.relocalization_poses)} relocalization poses to {self.tinynav_db_path}") |
| 603 | logging.info(f"Failed relocalizations count: {len(self.failed_relocalizations)}") |
| 604 | |
| 605 | self._save_completed = True |
| 606 | |
| 607 | def destroy_node(self): |
| 608 | try: |
| 609 | self.save_relocalization_poses() |
| 610 | self.nav_temp_db.close() |
| 611 | self.db.close() |
| 612 | super().destroy_node() |
| 613 | except Exception: |
| 614 | # Ignore errors during destruction as resources may already be freed |
| 615 | pass |
| 616 | |
| 617 | |
| 618 | def compute_transform_from_map_to_odom(self): |
| 619 | """ |
| 620 | Solve the optmization problem. |
| 621 | """ |
| 622 | relative_pose_constraint = [] |
| 623 | optimized_parameters = { |
| 624 | 0 : np.eye(4) if self.T_from_map_to_odom is None else self.T_from_map_to_odom, |
| 625 | 1 : np.eye(4), |
| 626 | } |
| 627 | constant_pose_index_dict = { 1: True } |
| 628 | for timestamp, pose in self.relocalization_poses.items(): |
| 629 | if timestamp in self.pose_graph_used_pose: |
| 630 | camera_in_map_world = pose |
| 631 | camera_in_odom_world = self.pose_graph_used_pose[timestamp] |
| 632 | observation_T_from_map_to_odom = camera_in_odom_world @ np.linalg.inv(camera_in_map_world) |
| 633 | weight = self.relocalization_pose_weights[timestamp] |
| 634 | |
| 635 | relative_pose_constraint.append((0, 1, observation_T_from_map_to_odom, weight * np.array([10.0, 10.0, 10.0]), weight * np.array([10.0, 10.0, 10.0]))) |
| 636 | relative_pose_constraint = select_fusion_constraints(relative_pose_constraint) |
| 637 | optimized_parameters = pose_graph_solve(optimized_parameters, relative_pose_constraint, constant_pose_index_dict, max_iteration_num = 1000) |
| 638 | self.T_from_map_to_odom = optimized_parameters[0] |
| 639 | |
| 640 | def _publish_global_plan(self, paths_in_map: np.ndarray): |
| 641 | path_msg = Path() |
| 642 | path_msg.header.stamp = self.get_clock().now().to_msg() |
| 643 | path_msg.header.frame_id = "map" |
| 644 | for x, y, z in paths_in_map: |
| 645 | pose = PoseStamped() |
| 646 | pose.header = path_msg.header |
| 647 | pose.pose.position.x = x |
| 648 | pose.pose.position.y = y |
| 649 | pose.pose.position.z = z |
| 650 | pose.pose.orientation.w = 1.0 |
| 651 | path_msg.poses.append(pose) |
| 652 | self.global_plan_pub.publish(path_msg) |
| 653 | |
| 654 | def publish_nav_progress(self, percent, path_remaining_m, path_total_m, estimated_remaining_s): |
nothing calls this directly
no test coverage detected