optimal

test/plugins/test_local_planner.cpp

@@ -20,6 +20,7 @@
  *********************************************************************/
 #include <algorithm>
 #include <cmath>
+#include <cstddef>
 #include <limits>
 #include <memory>
 #include <stdexcept>
@@ -30,7 +31,8 @@
 #include <boost/config.hpp>
 #include <boost/dll/alias.hpp>
 
-#include <robot_nav_core/base_local_planner.h>
+#include <robot_nav_2d_utils/conversions.h>
+#include <robot_nav_core2/local_planner.h>
 
 namespace move_base2
 {
@@ -44,7 +46,7 @@ constexpr double kBaseYawRate = 0.40;  ///< [rad/s]
  * @class TestLocalPlanner
  * @brief Local planner giả, hành vi cố định theo tham số dựng.
  */
-class TestLocalPlanner : public robot_nav_core::BaseLocalPlanner
+class TestLocalPlanner : public robot_nav_core2::LocalPlanner
 {
 public:
   enum class Behavior
@@ -60,46 +62,56 @@ public:
   {
   }
 
-  void initialize(std::string name, tf3::BufferCore* /*tf*/,
-                  robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
+  void initialize(robot::NodeHandle& /*parent*/, const std::string& name,
+                  std::shared_ptr<tf3::BufferCore> /*tf*/,
+                  robot_costmap_2d::Costmap2DROBOT* /*costmap*/) override
   {
     // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
-    name_ = std::move(name);
+    name_ = name;
   }
 
-  bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override
   {
-    return !plan.empty();
+    saw_goal_ = true;
   }
 
-  void getPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
+  void setPlan(const robot_nav_2d_msgs::Path2D& path) override
   {
-    path.clear();
+    plan_size_ = path.poses.size();
   }
 
-  void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
+  void getPlan(robot_nav_2d_msgs::Path2D& path) override
   {
-    path.clear();
+    path = robot_nav_2d_msgs::Path2D();
   }
 
-  bool computeVelocityCommands(const robot_geometry_msgs::Twist& velocity,
-                               robot_geometry_msgs::Twist& cmd_vel) override
+  void getGlobalPlan(robot_nav_2d_msgs::Path2D& path) override
   {
+    path = robot_nav_2d_msgs::Path2D();
+  }
+
+  robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(
+      const robot_nav_2d_msgs::Pose2DStamped& /*pose*/,
+      const robot_nav_2d_msgs::Twist2D& velocity) override
+  {
+    robot_nav_2d_msgs::Twist2DStamped cmd;
+
     switch (behavior_)
     {
       case Behavior::kThrow:
         throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
       case Behavior::kNoCommand:
-        return false;
+        // Gen-2 không có cờ thành công/thất bại: "không sinh được lệnh" biểu đạt bằng exception.
+        throw std::runtime_error("TestLocalPlanner: không sinh được lệnh");
       case Behavior::kNaN:
-        cmd_vel.linear.x = std::numeric_limits<double>::quiet_NaN();
-        return true;
+        cmd.velocity.x = std::numeric_limits<double>::quiet_NaN();
+        return cmd;
       case Behavior::kOk:
       case Behavior::kRefusesLimits:
         break;
     }
 
-    double linear = kBaseSpeed + velocity.linear.x;
+    double linear = kBaseSpeed + velocity.x;
     if (has_limit_forward_)
     {
       linear = std::min(linear, limit_forward_);
@@ -111,12 +123,13 @@ public:
       yaw = std::min(yaw, limit_angular_);
     }
 
-    cmd_vel.linear.x = linear;
-    cmd_vel.angular.z = yaw;
-    return true;
+    cmd.velocity.x = linear;
+    cmd.velocity.theta = yaw;
+    return cmd;
   }
 
-  bool isGoalReached() override
+  bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& /*pose*/,
+                     const robot_nav_2d_msgs::Twist2D& /*velocity*/) override
   {
     return false;
   }
@@ -168,6 +181,8 @@ public:
 private:
   Behavior behavior_;
   std::string name_;
+  std::size_t plan_size_ = 0;
+  bool saw_goal_ = false;
   double limit_forward_ = 0.0;   ///< [m/s]
   double limit_backward_ = 0.0;  ///< [m/s], âm
   double limit_angular_ = 0.0;   ///< [rad/s]
@@ -175,32 +190,32 @@ private:
   bool has_limit_angular_ = false;
 };
 
-robot_nav_core::BaseLocalPlanner::Ptr createOk()
+robot_nav_core2::LocalPlanner::Ptr createOk()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createSecondary()
+robot_nav_core2::LocalPlanner::Ptr createSecondary()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createNoCommand()
+robot_nav_core2::LocalPlanner::Ptr createNoCommand()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNoCommand);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createNaN()
+robot_nav_core2::LocalPlanner::Ptr createNaN()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNaN);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createThrowing()
+robot_nav_core2::LocalPlanner::Ptr createThrowing()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kThrow);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits()
+robot_nav_core2::LocalPlanner::Ptr createRefusingLimits()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
 }