| 648 | } |
| 649 | |
| 650 | void AmclNode::do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) { |
| 651 | auto pose = Sophus::SE2d{}; |
| 652 | tf2::convert(message->pose.pose, pose); |
| 653 | |
| 654 | auto covariance = Eigen::Matrix3d{}; |
| 655 | tf2::covarianceRowMajorToEigen(message->pose.covariance, covariance); |
| 656 | |
| 657 | last_known_estimate_ = std::make_pair(pose, covariance); |
| 658 | last_known_odom_transform_in_map_.reset(); |
| 659 | initialize_from_estimate(last_known_estimate_.value()); |
| 660 | } |
| 661 | |
| 662 | void AmclNode::global_localization_callback( |
| 663 | [[maybe_unused]] std::shared_ptr<rmw_request_id_t> request_header, |