toid_behaviors/include/toid_behaviors/simple_move.hpp

@@ -37,7 +37,6 @@ public:
     odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
       odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
         std::lock_guard lock(mut_);
-        current_pose_ = msg.pose.pose;
         current_vel_ = msg.twist.twist;
       });
     control_duration_ = 1.0 / this->cycle_frequency_;
@@ -65,28 +64,40 @@ public:
   nav2_behaviors::ResultStatus onRun(
     const std::shared_ptr<const typename ActionT::Goal> command) override
   {
-    geometry_msgs::msg::Pose pose;
+    geometry_msgs::msg::PoseStamped pose;
     geometry_msgs::msg::Twist vel;
     {
       std::lock_guard lock(mut_);
-      pose = current_pose_;
       vel = current_vel_;
     }
-    return onStart(command, pose, vel);
+
+    if (!nav2_util::getCurrentPose(
+          pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
+          this->transform_tolerance_)) {
+      return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED};
+    }
+
+    return onStart(command, pose.pose, vel);
   }
 
   nav2_behaviors::ResultStatus onCycleUpdate() override
   {
-    geometry_msgs::msg::Pose pose;
+    geometry_msgs::msg::PoseStamped pose;
     geometry_msgs::msg::Twist vel;
     geometry_msgs::msg::Twist out_vel;
     auto vel_p = std::make_unique<geometry_msgs::msg::TwistStamped>();
     {
       std::lock_guard lock(mut_);
-      pose = current_pose_;
       vel = current_vel_;
     }
-    nav2_behaviors::ResultStatus r = updateVel(pose, vel, out_vel);
+
+    if (!nav2_util::getCurrentPose(
+          pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
+          this->transform_tolerance_)) {
+      return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED};
+    }
+
+    nav2_behaviors::ResultStatus r = updateVel(pose.pose, vel, out_vel);
     vel_p->twist = out_vel;
     vel_p->header.stamp = this->clock_->now();
     vel_p->header.frame_id = this->robot_base_frame_;