first commit

src/config/move_base2_config.cpp

@@ -0,0 +1,261 @@
+/*********************************************************************
+ * move_base2 — đọc và validate cấu hình runtime.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/config/move_base2_config.h>
+
+#include <cmath>
+#include <sstream>
+
+#include <robot/robot.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// Đọc một khoá double; thiếu khoá thì giữ nguyên default và nói rõ khoá nào bị thiếu.
+void readDouble(robot::NodeHandle& nh, const std::string& key, double& value)
+{
+  if (!nh.hasParam(key))
+  {
+    robot::log_warning("[move_base2] thiếu param '%s', dùng default %.4f", key.c_str(), value);
+    return;
+  }
+  nh.param(key, value, value);
+}
+
+void readInt(robot::NodeHandle& nh, const std::string& key, int& value)
+{
+  if (!nh.hasParam(key))
+  {
+    robot::log_warning("[move_base2] thiếu param '%s', dùng default %d", key.c_str(), value);
+    return;
+  }
+  nh.param(key, value, value);
+}
+
+void readBool(robot::NodeHandle& nh, const std::string& key, bool& value)
+{
+  if (!nh.hasParam(key))
+  {
+    robot::log_warning("[move_base2] thiếu param '%s', dùng default %s", key.c_str(),
+                       value ? "true" : "false");
+    return;
+  }
+  nh.param(key, value, value);
+}
+
+void readString(robot::NodeHandle& nh, const std::string& key, std::string& value)
+{
+  if (!nh.hasParam(key))
+  {
+    robot::log_warning("[move_base2] thiếu param '%s', dùng default '%s'", key.c_str(),
+                       value.c_str());
+    return;
+  }
+  nh.param(key, value, value);
+}
+
+/// Đọc một binding profile từ namespace con cùng tên.
+void readBinding(robot::NodeHandle& nh, const std::string& ns, ProfileBinding& binding)
+{
+  robot::NodeHandle profile_nh(nh, ns);
+  readString(profile_nh, "base_global_planner", binding.global_planner_name);
+  readString(profile_nh, "base_local_planner", binding.local_planner_name);
+  readDouble(profile_nh, "xy_goal_tolerance", binding.default_xy_tolerance);
+  readDouble(profile_nh, "yaw_goal_tolerance", binding.default_yaw_tolerance);
+}
+
+bool validateBinding(const ProfileBinding& binding, const char* name, std::string& error)
+{
+  if (binding.local_planner_name.empty())
+  {
+    // Không đặt là hợp lệ: deployment có thể không dùng profile đó. Nhưng nếu đã đặt planner thì
+    // sai số phải hợp lệ, vì chúng đi thẳng vào điều kiện dừng.
+    return true;
+  }
+  if (!std::isfinite(binding.default_xy_tolerance) || binding.default_xy_tolerance <= 0.0)
+  {
+    error = std::string(name) + ".xy_goal_tolerance phải > 0 [m]";
+    return false;
+  }
+  if (!std::isfinite(binding.default_yaw_tolerance) || binding.default_yaw_tolerance <= 0.0)
+  {
+    error = std::string(name) + ".yaw_goal_tolerance phải > 0 [rad]";
+    return false;
+  }
+  return true;
+}
+
+/// Đọc cấu hình đường vào cảm biến từ namespace con `sensors`.
+void readSensors(robot::NodeHandle& nh, SensorGatewayConfig& sensors)
+{
+  robot::NodeHandle sensors_nh(nh, "sensors");
+  readBool(sensors_nh, "laser_sor_enabled", sensors.laser_sor_enabled);
+  readInt(sensors_nh, "laser_sor_mean_k", sensors.laser_sor_mean_k);
+  readDouble(sensors_nh, "laser_sor_stddev_mul", sensors.laser_sor_stddev_mul);
+}
+
+void describeBinding(std::ostringstream& out, const char* name, const ProfileBinding& binding)
+{
+  out << "  " << name << ": global='" << binding.global_planner_name << "' local='"
+      << binding.local_planner_name << "' xy=" << binding.default_xy_tolerance
+      << " m yaw=" << binding.default_yaw_tolerance << " rad\n";
+}
+
+}  // namespace
+
+void MoveBase2Config::fromNodeHandle(robot::NodeHandle& nh)
+{
+  readDouble(nh, "controller_frequency", controller_frequency);
+  readDouble(nh, "planner_frequency", planner_frequency);
+  readDouble(nh, "planner_timeout", planner_timeout);
+
+  readDouble(nh, "planner_patience", state_machine.planner_patience);
+  readDouble(nh, "controller_patience", state_machine.controller_patience);
+  readDouble(nh, "oscillation_timeout", state_machine.oscillation_timeout);
+  readDouble(nh, "oscillation_distance", state_machine.oscillation_distance);
+  readDouble(nh, "action_patience", state_machine.action_patience);
+  readInt(nh, "max_planning_retries", state_machine.max_planning_retries);
+  readBool(nh, "recovery_behavior_enabled", state_machine.recovery_enabled);
+
+  readDouble(nh, "max_vel_x", velocity.max_vel_x);
+  readDouble(nh, "min_vel_x", velocity.min_vel_x);
+  readDouble(nh, "max_vel_theta", velocity.max_vel_theta);
+  readDouble(nh, "acc_lim_x", velocity.max_accel_x);
+  readDouble(nh, "acc_lim_theta", velocity.max_accel_theta);
+
+  readSensors(nh, sensors);
+
+  readBinding(nh, "position", position);
+  readBinding(nh, "docking", docking);
+  readBinding(nh, "go_straight", go_straight);
+  readBinding(nh, "rotate", rotate);
+
+  readString(nh, "recovery_namespace", recovery_namespace);
+  readString(nh, "action_namespace", action_namespace);
+  readString(nh, "mission_namespace", mission_namespace);
+  readString(nh, "global_frame", global_frame);
+  readString(nh, "robot_base_frame", robot_base_frame);
+
+  // recovery_behavior_count KHÔNG đọc từ YAML: nó là số behavior thực sự nạp được, do
+  // RecoveryRunner báo lại sau khi configure. Đọc từ config thì một behavior hỏng sẽ khiến state
+  // machine tin là vẫn còn đường phục hồi.
+}
+
+bool MoveBase2Config::validate(std::string& error) const
+{
+  if (!std::isfinite(controller_frequency) || controller_frequency <= 0.0)
+  {
+    error = "controller_frequency phải > 0 [Hz]";
+    return false;
+  }
+  if (controller_frequency > 200.0)
+  {
+    error = "controller_frequency > 200 Hz — nhịp này không thực tế cho một control loop có costmap";
+    return false;
+  }
+  if (!std::isfinite(planner_frequency) || planner_frequency < 0.0)
+  {
+    error = "planner_frequency phải >= 0 [Hz] (0 = chỉ lập plan khi cần)";
+    return false;
+  }
+  if (!std::isfinite(planner_timeout))
+  {
+    error = "planner_timeout không hữu hạn [s]";
+    return false;
+  }
+
+  if (recovery_namespace.empty())
+  {
+    error = "recovery_namespace rỗng";
+    return false;
+  }
+  if (global_frame.empty() || robot_base_frame.empty())
+  {
+    error = "global_frame và robot_base_frame không được rỗng";
+    return false;
+  }
+  if (global_frame == robot_base_frame)
+  {
+    error = "global_frame trùng robot_base_frame — pose robot sẽ luôn là gốc toạ độ";
+    return false;
+  }
+
+  if (!validateBinding(position, "position", error) ||
+      !validateBinding(docking, "docking", error) ||
+      !validateBinding(go_straight, "go_straight", error) ||
+      !validateBinding(rotate, "rotate", error))
+  {
+    return false;
+  }
+
+  if (position.local_planner_name.empty() && docking.local_planner_name.empty() &&
+      go_straight.local_planner_name.empty() && rotate.local_planner_name.empty())
+  {
+    error = "không profile nào có base_local_planner — runtime sẽ từ chối mọi yêu cầu";
+    return false;
+  }
+
+  if (!velocity.validate(error))
+  {
+    return false;
+  }
+
+  if (!sensors.validate(error))
+  {
+    return false;
+  }
+
+  // Sau cùng: struct con. Thứ tự này có chủ đích — báo lỗi ở tầng cụ thể nhất trước, để thông báo
+  // nói đúng khoá YAML mà người vận hành cần sửa, chứ không phải một ràng buộc phái sinh.
+  //
+  // Lưu ý ràng buộc thứ tự KHỞI TẠO: `state_machine.recovery_behavior_count` KHÔNG đến từ YAML mà
+  // là số behavior RecoveryRunner nạp được thật. Bên gọi phải điền nó trước khi gọi hàm này —
+  // xem @ref MoveBase2Config::validate trong header.
+  if (!state_machine.validate(error))
+  {
+    return false;
+  }
+
+  return true;
+}
+
+std::string MoveBase2Config::describe() const
+{
+  std::ostringstream out;
+  out << "move_base2 config:\n";
+  out << "  controller_frequency: " << controller_frequency << " Hz\n";
+  out << "  planner_frequency: " << planner_frequency << " Hz\n";
+  out << "  planner_timeout: " << planner_timeout << " s\n";
+  out << "  frames: global='" << global_frame << "' base='" << robot_base_frame << "'\n";
+  out << "  namespaces: recovery='" << recovery_namespace << "' actions='" << action_namespace
+      << "' mission='" << mission_namespace << "'\n";
+  describeBinding(out, "position", position);
+  describeBinding(out, "docking", docking);
+  describeBinding(out, "go_straight", go_straight);
+  describeBinding(out, "rotate", rotate);
+  out << state_machine.describe();
+  out << velocity.describe();
+  out << sensors.describe();
+  return out.str();
+}
+
+ControlLoopConfig MoveBase2Config::toControlLoopConfig() const
+{
+  ControlLoopConfig config;
+  config.state_machine = state_machine;
+  config.velocity = velocity;
+  config.nominal_control_period =
+      controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05;  // [s]
+  config.robot_base_frame = robot_base_frame;
+  config.position = position;
+  config.docking = docking;
+  config.go_straight = go_straight;
+  config.rotate = rotate;
+  return config;
+}
+
+}  // namespace move_base2