fix

usdot-fhwa-stol · Jul 11, 2023 · 6aa8ead · 6aa8ead
1 parent fd5c000
commit 6aa8ead
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git a/core_perception/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/core_perception/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -52,7 +52,7 @@ class EKFLocalizer: public carma_ros2_utils::CarmaLifecycleNode
     rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_yaw_bias_;                                //!< @brief ekf estimated yaw bias publisher
 
     rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;     //!< @brief initial pose subscriber
-    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_pose_with_cov_;   //!< @brief measurement pose with covariance subscriber
+    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pose_with_cov_;   //!< @brief measurement pose with covariance subscriber
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_pose_;                          //!< @brief measurement pose subscriber
     rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr sub_twist_with_cov_; //!< @brief measurement twist with covariance subscriber
     rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_twist_;                        //!< @brief measurement twist subscriber

diff --git a/core_perception/ekf_localizer/src/ekf_localizer.cpp b/core_perception/ekf_localizer/src/ekf_localizer.cpp
@@ -114,8 +114,9 @@ namespace ekf_localizer{
         pub_twist_ = create_publisher<geometry_msgs::msg::TwistStamped>("ekf_twist", 1);
         pub_twist_cov_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>("ekf_twist_with_covariance", 1);
         pub_yaw_bias_ = create_publisher<std_msgs::msg::Float64>("estimated_yaw_bias", 1);
-        sub_initialpose_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, std::placeholders::_1));
-        sub_pose_with_cov_ = create_subscription<geometry_msgs::msg::PoseStamped>("in_pose", 1, std::bind(&EKFLocalizer::callbackPose, this, std::placeholders::_1));
+        sub_initialpose_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, std::placeholders::_1));
+        sub_pose_with_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, std::placeholders::_1)); 
+        sub_pose_ = create_subscription<geometry_msgs::msg::PoseStamped>("in_pose", 1, std::bind(&EKFLocalizer::callbackPose, this, std::placeholders::_1));
         sub_twist_with_cov_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>("in_twist_with_covariance", 1, std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, std::placeholders::_1));
         sub_twist_ = create_subscription<geometry_msgs::msg::TwistStamped>("in_twist", 1, std::bind(&EKFLocalizer::callbackTwist, this, std::placeholders::_1));