diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index a847d64..22cba75 100755 --- a/config/costmap_common_params.yaml +++ b/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 \ No newline at end of file diff --git a/examples/NavigationExample/NavigationExample.csproj b/examples/NavigationExample/NavigationExample.csproj index 0d40596..48dbb2c 100644 --- a/examples/NavigationExample/NavigationExample.csproj +++ b/examples/NavigationExample/NavigationExample.csproj @@ -5,6 +5,9 @@ linux-x64 true + + + Always diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 48015e8..e078b89 100644 --- a/examples/NavigationExample/Program.cs +++ b/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 { + /// Thông tin map đọc từ file YAML (maze.yaml). + 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; + } /// /// C# P/Invoke wrapper for Navigation C API /// @@ -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 { + /// + /// Đọ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. + /// + 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; + } + } + + /// + /// Đọ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. + /// + 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(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 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(); } } } diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index c9c3037..654d9a6 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/src/APIs/c_api/CMakeLists.txt b/src/APIs/c_api/CMakeLists.txt index 165b552..efeb0da 100644 --- a/src/APIs/c_api/CMakeLists.txt +++ b/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 diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index 0b4c51b..6811104 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/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 diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 9d69a99..0bd1393 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/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 (...) { diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index 23ce2c8..c02e6a0 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/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; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp index c07c1c2..401e9b6 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp +++ b/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(); diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index eb52edc..3d621de 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit eb52edc6e81537f408c4e343887cd4cbb0ed13a2 +Subproject commit 3d621de80918df34c8e5ec67e95843bc9225e825 diff --git a/src/Libraries/robot_nav_2d_utils/CMakeLists.txt b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt index b6eb228..b188e59 100755 --- a/src/Libraries/robot_nav_2d_utils/CMakeLists.txt +++ b/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} diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index 8ce9909..66a860f 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -35,6 +35,17 @@ namespace move_base { + template + 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 MoveBaseActionServer; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 7ce4f58..7c31ce2 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -32,7 +32,6 @@ #include #include - 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(); }