update

config/costmap_common_params.yaml

@@ -61,113 +61,3 @@ obstacles:
     max_obstacle_height: 0.25
 
 
-global_costmap:
-  robot_base_frame: base_footprint
-  transform_tolerance: 1.0
-  obstacle_range: 3.0
-  #mark_threshold: 1
-  publish_voxel_map: true
-  footprint_padding: 0.0
-
-  navigation_map:
-    map_topic: /map
-    map_pkg: managerments
-    map_file: maze
-
-  virtual_walls_map:
-    map_topic: /virtual_walls/map
-    namespace: /virtual_walls
-    map_pkg: managerments
-    map_file: maze
-    use_maximum: true
-    lethal_cost_threshold: 100
-
-  obstacles:
-    observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-    f_scan_marking:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    f_scan_clearing:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_marking:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_clearing:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-local_costmap:
-  robot_base_frame: base_footprint
-  transform_tolerance: 1.0
-  obstacle_range: 3.0
-  #mark_threshold: 1
-  publish_voxel_map: true
-  footprint_padding: 0.0
-
-  navigation_map:
-    map_topic: /map
-    map_pkg: managerments
-    map_file: maze
-
-  virtual_walls_map:
-    map_topic: /virtual_walls/map
-    namespace: /virtual_walls
-    map_pkg: managerments
-    map_file: maze
-    use_maximum: true
-    lethal_cost_threshold: 100
-
-  obstacles:
-    observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-    f_scan_marking:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    f_scan_clearing:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_marking:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_clearing:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25

examples/NavigationExample/NavigationExample.csproj

@@ -5,6 +5,9 @@
     <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
+  <ItemGroup>
+    <PackageReference Include="SixLabors.ImageSharp" Version="3.1.5" />
+  </ItemGroup>
   <ItemGroup>
     <None Include="libnav_c_api.so">
       <CopyToOutputDirectory>Always</CopyToOutputDirectory>

examples/NavigationExample/Program.cs

@@ -1,9 +1,24 @@
 using System;
+using System.Globalization;
+using System.IO;
 using System.Runtime.InteropServices;
 using System.Runtime.CompilerServices;
+using System.Text.RegularExpressions;
+using SixLabors.ImageSharp;
+using SixLabors.ImageSharp.PixelFormats;
 
 namespace NavigationExample
 {
+    /// <summary>Thông tin map đọc từ file YAML (maze.yaml).</summary>
+    public struct MazeMapConfig
+    {
+        public string ImagePath;       // đường dẫn đầy đủ tới file ảnh
+        public float Resolution;
+        public double OriginX, OriginY, OriginZ;
+        public int Negate;            // 0 hoặc 1
+        public double OccupiedThresh;
+        public double FreeThresh;
+    }
     /// <summary>
     /// C# P/Invoke wrapper for Navigation C API
     /// </summary>
@@ -308,7 +323,7 @@ namespace NavigationExample
 
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
         [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_straight_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance);
+        public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);
 
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
         [return: MarshalAs(UnmanagedType.I1)]
@@ -370,6 +385,105 @@ namespace NavigationExample
     // ============================================================================
     class Program
     {
+        /// <summary>
+        /// Đọc file maze.yaml (image, resolution, origin, negate, occupied_thresh, free_thresh).
+        /// ImagePath được resolve tuyệt đối theo thư mục chứa file yaml.
+        /// </summary>
+        static bool TryLoadMazeYaml(string yamlPath, out MazeMapConfig config)
+        {
+            config = default;
+            if (string.IsNullOrEmpty(yamlPath) || !File.Exists(yamlPath))
+                return false;
+            string dir = Path.GetDirectoryName(Path.GetFullPath(yamlPath)) ?? "";
+            try
+            {
+                string[] lines = File.ReadAllLines(yamlPath);
+                foreach (string line in lines)
+                {
+                    string trimmed = line.Trim();
+                    if (trimmed.Length == 0 || trimmed.StartsWith("#")) continue;
+                    int colon = trimmed.IndexOf(':');
+                    if (colon <= 0) continue;
+                    string key = trimmed.Substring(0, colon).Trim();
+                    string value = trimmed.Substring(colon + 1).Trim();
+                    if (key == "image")
+                        config.ImagePath = Path.Combine(dir, value);
+                    else if (key == "resolution" && float.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out float res))
+                        config.Resolution = res;
+                    else if (key == "origin")
+                    {
+                        var m = Regex.Match(value, @"\[\s*([-\d.]+\s*),\s*([-\d.]+\s*),\s*([-\d.]+)\s*\]");
+                        if (m.Success)
+                        {
+                            config.OriginX = double.Parse(m.Groups[1].Value, CultureInfo.InvariantCulture);
+                            config.OriginY = double.Parse(m.Groups[2].Value, CultureInfo.InvariantCulture);
+                            config.OriginZ = double.Parse(m.Groups[3].Value, CultureInfo.InvariantCulture);
+                        }
+                    }
+                    else if (key == "negate" && int.TryParse(value, out int neg))
+                        config.Negate = neg;
+                    else if (key == "occupied_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ot))
+                        config.OccupiedThresh = ot;
+                    else if (key == "free_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ft))
+                        config.FreeThresh = ft;
+                }
+                return !string.IsNullOrEmpty(config.ImagePath);
+            }
+            catch
+            {
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// Đọc file ảnh PNG và chuyển pixel sang byte[] occupancy (0=free, 100=occupied, 255=unknown).
+        /// Dùng negate, occupied_thresh, free_thresh từ maze.yaml nếu cung cấp.
+        /// </summary>
+        static byte[] LoadMazePixelsAsOccupancy(string imagePath, out int width, out int height,
+            int negate = 0, double occupiedThresh = 0.65, double freeThresh = 0.196)
+        {
+            width = 0;
+            height = 0;
+            if (string.IsNullOrEmpty(imagePath) || !File.Exists(imagePath))
+            {
+                LogError($"File not found: {imagePath}");
+                return null;
+            }
+            try
+            {
+                using var image = Image.Load<Rgba32>(imagePath);
+                int w = image.Width;
+                int h = image.Height;
+                width = w;
+                height = h;
+                byte[] data = new byte[w * h];
+                image.ProcessPixelRows(accessor =>
+                {
+                    for (int y = 0; y < accessor.Height; y++)
+                    {
+                        Span<Rgba32> row = accessor.GetRowSpan(y);
+                        for (int x = 0; x < row.Length; x++)
+                        {
+                            ref readonly Rgba32 p = ref row[x];
+                            double gray = (p.R + p.G + p.B) / 255.0 / 3.0;
+                            if (negate != 0) gray = 1.0 - gray;
+                            byte occ;
+                            if (gray <= freeThresh) occ = 0;
+                            else if (gray >= occupiedThresh) occ = 100;
+                            else occ = 255; // unknown
+                            data[y * w + x] = occ;
+                        }
+                    }
+                });
+                return data;
+            }
+            catch (Exception ex)
+            {
+                LogError($"Load image failed: {ex.Message}");
+                return null;
+            }
+        }
+
         // Helper method để hiển thị file và line number tự động
         static void LogError(string message,
             [CallerFilePath] string filePath = "",
@@ -459,8 +573,7 @@ namespace NavigationExample
                 Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
                 NavigationAPI.navigation_free_feedback(ref feedback);
             }
-
-            System.Threading.Thread.Sleep(1000);   
+  
             IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
             NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
             NavigationAPI.LaserScan fscanHandle;
@@ -480,7 +593,6 @@ namespace NavigationExample
             fscanHandle.intensities_count = new UIntPtr(5);
             NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan",  fscanHandle);
 
-
             IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan");
             NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
             NavigationAPI.LaserScan bscanHandle;
@@ -500,8 +612,6 @@ namespace NavigationExample
             bscanHandle.intensities_count = new UIntPtr(5);
             NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan",  bscanHandle);
 
-            System.Threading.Thread.Sleep(1000); 
-
             IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom");
             NavigationAPI.Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
             NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry();
@@ -539,18 +649,53 @@ namespace NavigationExample
             odometryHandle.twist.covariance_count = new UIntPtr((uint)twist_covariance.Length);
             NavigationAPI.navigation_add_odometry(navHandle, "odometry",  odometryHandle);
 
-            System.Threading.Thread.Sleep(1000);  
-            // Add static map
+            // Add static map: đọc maze.yaml rồi load ảnh và cập nhật mapMetaData
+            string mapYamlPath = args.Length > 0 ? args[0] : Path.Combine(
+                "/home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws/src/Managerments/maps/maze", "maze.yaml");
+            int mapWidth, mapHeight;
+            byte[] data;
+            MazeMapConfig mapConfig;
+            if (TryLoadMazeYaml(mapYamlPath, out mapConfig))
+            {
+                data = LoadMazePixelsAsOccupancy(mapConfig.ImagePath, out mapWidth, out mapHeight,
+                    mapConfig.Negate, mapConfig.OccupiedThresh, mapConfig.FreeThresh);
+                if (data == null)
+                {
+                    mapWidth = 3;
+                    mapHeight = 10;
+                    data = new byte[30];
+                    for (int i = 0; i < data.Length; i++) data[i] = 100;
+                    Console.WriteLine("YAML loaded but image failed; using default map 3x10");
+                }
+                else
+                {
+                    Console.WriteLine("Loaded map from {0}: {1}x{2}, resolution={3}, origin=({4},{5},{6})",
+                        mapConfig.ImagePath, mapWidth, mapHeight, mapConfig.Resolution,
+                        mapConfig.OriginX, mapConfig.OriginY, mapConfig.OriginZ);
+                }
+            }
+            else
+            {
+                mapWidth = 3;
+                mapHeight = 10;
+                data = new byte[30];
+                for (int i = 0; i < data.Length; i++) data[i] = 100;
+                mapConfig = default;
+                mapConfig.Resolution = 0.05f;
+                mapConfig.OriginX = mapConfig.OriginY = mapConfig.OriginZ = 0.0;
+                Console.WriteLine("maze.yaml not found, using default map 3x10");
+            }
+
             NavigationAPI.Time mapLoadTime = NavigationAPI.time_create();
             NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData();
             mapMetaData.map_load_time = mapLoadTime;
-            mapMetaData.resolution = 0.05f;
-            mapMetaData.width = 3;
-            mapMetaData.height = 10;
+            mapMetaData.resolution = mapConfig.Resolution;
+            mapMetaData.width = (uint)mapWidth;
+            mapMetaData.height = (uint)mapHeight;
             mapMetaData.origin = new NavigationAPI.Pose();
-            mapMetaData.origin.position.x = 0.0;
-            mapMetaData.origin.position.y = 0.0;
-            mapMetaData.origin.position.z = 0.0;
+            mapMetaData.origin.position.x = mapConfig.OriginX;
+            mapMetaData.origin.position.y = mapConfig.OriginY;
+            mapMetaData.origin.position.z = mapConfig.OriginZ;
             mapMetaData.origin.orientation.x = 0.0;
             mapMetaData.origin.orientation.y = 0.0;
             mapMetaData.origin.orientation.z = 0.0;
@@ -559,10 +704,6 @@ namespace NavigationExample
             IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map");
             occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
             occupancyGrid.info = mapMetaData;
-            byte[] data = new byte[30];
-            for (int i = 0; i < data.Length; i++) {
-                data[i] = 100;
-            }
             occupancyGrid.data = Marshal.AllocHGlobal(sizeof(byte) * data.Length);
             Marshal.Copy(data, 0, occupancyGrid.data, data.Length);
             occupancyGrid.data_count = new UIntPtr((uint)data.Length);
@@ -574,7 +715,7 @@ namespace NavigationExample
     
             NavigationAPI.navigation_add_static_map(navHandle, "/map",  occupancyGrid);
 
-            System.Threading.Thread.Sleep(1000);
+            System.Threading.Thread.Sleep(500);
 
             NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
             if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
@@ -584,8 +725,12 @@ namespace NavigationExample
                     NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
             }            
             // Cleanup
+            NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
+
             NavigationAPI.navigation_destroy(navHandle);
             NavigationAPI.tf_listener_destroy(tfHandle);
+            Console.WriteLine("Press any key to exit...");
+            Console.ReadKey();
         }
     }
 }

src/APIs/c_api/CMakeLists.txt

@@ -13,9 +13,10 @@ endif()
 # Find Boost (filesystem needed for plugin path / boost::dll usage)
 find_package(Boost REQUIRED COMPONENTS filesystem system)
 
-# Dependencies
+# Dependencies (robot_nav_2d_utils_conversions provides poseStampedToPose2D used by move_base_core/navigation.h)
 set(PACKAGES_DIR
     move_base_core
+    robot_nav_2d_utils
     tf3
     robot_time
     robot_cpp

src/APIs/c_api/include/nav_c_api.h

@@ -218,12 +218,10 @@ extern "C"
   /**
    * @brief Move straight toward the target position
    * @param handle Navigation handle
-   * @param goal Target pose
-   * @param xy_goal_tolerance Acceptable positional error (meters)
+   * @param distance Distance to move (meters)
    * @return true if command issued successfully
    */
-  bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped goal,
-                                   double xy_goal_tolerance);
+  bool navigation_move_straight_to(NavigationHandle handle, const double distance);
 
   /**
    * @brief Rotate in place to align with target orientation

src/APIs/c_api/src/nav_c_api.cpp

@@ -354,8 +354,7 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker,
 //         return false;
 //     }
 
-extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped goal,
-                                            double xy_goal_tolerance)
+extern "C" bool navigation_move_straight_to(NavigationHandle handle, const double distance)
 {
   if (!handle.ptr)
   {
@@ -368,8 +367,11 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS
     if (!nav_ptr || !*nav_ptr)
       return false;
     auto *nav = nav_ptr->get();
-    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
-    return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
+    robot_geometry_msgs::PoseStamped pose;
+    if (!nav->getRobotPose(pose))
+      return false;
+    robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance);
+    return nav->moveStraightTo(goal);
   }
   catch (...)
   {

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp

@@ -9,8 +9,6 @@ void mkt_algorithm::diff::GoStraight::initialize(
   {
     nh_ = nh;
     nh_priv_ = robot::NodeHandle(nh, name);
-    robot::log_info_at(__FILE__, __LINE__, "nh_ : %s", nh_.getNamespace().c_str());
-    robot::log_info_at(__FILE__, __LINE__, "nh_priv_ : %s", nh_priv_.getNamespace().c_str());
     name_ = name;
     tf_ = tf;
     traj_ = traj;

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp

@@ -35,6 +35,16 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
   if (!initialized_)
   {
     robot::log_info_at(__FILE__, __LINE__, "Using parent name \"%s\"", name.c_str());
+    if(costmap_robot == nullptr)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Costmap2DROBOT is nullptr");
+      throw robot_nav_core2::LocalPlannerException("Costmap2DROBOT is nullptr");
+    }
+    if(tf == nullptr)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "TFListener is nullptr");
+      throw robot_nav_core2::LocalPlannerException("TFListener is nullptr");
+    }
     tf_ = tf;
     costmap_robot_ = costmap_robot;
     costmap_ = costmap_robot_->getCostmap();

src/Libraries/robot_nav_2d_utils/CMakeLists.txt

@@ -132,6 +132,11 @@ else()
   )
   target_link_libraries(${PROJECT_NAME}
       INTERFACE
+        ${PROJECT_NAME}_conversions
+        ${PROJECT_NAME}_path_ops
+        ${PROJECT_NAME}_polygons
+        ${PROJECT_NAME}_bounds
+        ${PROJECT_NAME}_tf_help
         ${PACKAGES_DIR}
         ${Boost_LIBRARIES}
         ${TF3_LIBRARY}

src/Navigations/Packages/move_base/include/move_base/move_base.h

@@ -35,6 +35,17 @@
 
 namespace move_base
 {
+  template <class T>
+  void move_parameter(robot::NodeHandle& old_h, robot::NodeHandle& new_h, std::string name,T& value)
+  {
+    if (!old_h.hasParam(name))
+      return;
+  
+    old_h.getParam(name, value);
+    new_h.setParam(name, value);
+  }
+
+
   // typedefs to help us out with the action server so that we don't hace to type so much
   typedef move_base::SimpleActionServer<robot_move_base_msgs::MoveBaseAction> MoveBaseActionServer;
 

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -32,7 +32,6 @@
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_nav_2d_utils/conversions.h>
 
-
 move_base::MoveBase::MoveBase()
     : initialized_(false),
       planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
@@ -54,6 +53,7 @@ move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
 
 move_base::MoveBase::~MoveBase()
 {
+  robot::log_warning("Destroying MoveBase instance...");
   if (as_ != NULL)
   { 
     as_->stop();
@@ -124,8 +124,16 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
   printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
   if (!initialized_)
   {
+    if(tf == nullptr)
+    {
+      robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__);
+      throw std::runtime_error("Failed to create the tf");
+    }
+    else
+    {
+      robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
+    }
     tf_ = tf;
-
     as_ = new MoveBaseActionServer("move_base", std::bind(&MoveBase::executeCb, this, std::placeholders::_1), true);
     setupActionServerCallbacks();
 
@@ -250,6 +258,11 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     try
     {
       controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
+      if(controller_costmap_robot_ == nullptr)
+      {
+        robot::log_error("[%s:%d]\n ERROR: controller_costmap_robot_ is nullptr", __FILE__, __LINE__);
+        throw std::runtime_error("Failed to create the controller_costmap_robot_");
+      }
       controller_costmap_robot_->pause();
       robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
     }
@@ -1128,6 +1141,12 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
     }
     
     robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
+    if(controller_costmap_robot_ == nullptr)
+    {
+      robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ is null!");
+      return false;
+    }
+
     as_->processGoal(action_goal);
     robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server");
   }
@@ -1771,7 +1790,7 @@ void move_base::MoveBase::resetState()
   // if we shutdown our costmaps when we're deactivated... we'll do that now
   if (shutdown_costmaps_)
   {
-    std::cout << "Stopping costmaps" << std::endl;
+    robot::log_info("Stopping costmaps");
     planner_costmap_robot_->stop();
     controller_costmap_robot_->stop();
   }