diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index e078b89..3b1a84b 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -497,13 +497,14 @@ namespace NavigationExample static void Main(string[] args) { - // Create TF listener + // Create TF listener (required for costmap and local planner; must be valid when calling navigation_initialize) NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create(); if (tfHandle.ptr == IntPtr.Zero) { LogError("Failed to create TF listener"); return; } + Console.WriteLine($"[NavigationExample] TF listener created, handle = 0x{tfHandle.ptr.ToInt64():X16}"); // Inject a static TF so costmap can immediately canTransform(map <-> base_link). // If you already publish TF from localization/odometry, you can remove this call. @@ -543,15 +544,35 @@ namespace NavigationExample return; } - // Initialize navigation + // Initialize navigation (passes TF to move_base; navigation keeps its own copy, so tfHandle can be destroyed later) if (!NavigationAPI.navigation_initialize(navHandle, tfHandle)) { - LogError("Failed to initialize navigation"); + LogError("Failed to initialize navigation (check native log for 'Invalid TF listener' or 'tf is nullptr')"); NavigationAPI.navigation_destroy(navHandle); NavigationAPI.tf_listener_destroy(tfHandle); return; } + while(true) + { + // Get navigation feedback + NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); + if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) + { + if(feedback.is_ready) + { + Console.WriteLine("Navigation is ready"); + break; + } + else + { + Console.WriteLine("Navigation is not ready"); + } + } + System.Threading.Thread.Sleep(100); + } + Console.WriteLine("[NavigationExample] Navigation initialized with TF successfully"); + // Set robot footprint NavigationAPI.Point[] footprint = new NavigationAPI.Point[] { @@ -562,17 +583,6 @@ namespace NavigationExample }; NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); - // Get navigation feedback - NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); - if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) - { - IntPtr stateStrPtr = NavigationAPI.navigation_state_to_string(feedback.navigation_state); - string stateStr = NavigationAPI.MarshalString(stateStrPtr); - NavigationAPI.nav_c_api_free_string(stateStrPtr); - string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str); - Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}"); - NavigationAPI.navigation_free_feedback(ref feedback); - } IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan"); NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId)); @@ -650,8 +660,7 @@ namespace NavigationExample NavigationAPI.navigation_add_odometry(navHandle, "odometry", odometryHandle); // 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"); + string mapYamlPath = "maze.yaml"; int mapWidth, mapHeight; byte[] data; MazeMapConfig mapConfig; @@ -715,8 +724,6 @@ namespace NavigationExample NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid); - System.Threading.Thread.Sleep(500); - NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped(); if (NavigationAPI.navigation_get_twist(navHandle, ref twist)) { diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 654d9a6..604760d 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/examples/NavigationExample/maze.png b/examples/NavigationExample/maze.png new file mode 100644 index 0000000..41d6fa1 Binary files /dev/null and b/examples/NavigationExample/maze.png differ diff --git a/examples/NavigationExample/maze.yaml b/examples/NavigationExample/maze.yaml new file mode 100644 index 0000000..79220e7 --- /dev/null +++ b/examples/NavigationExample/maze.yaml @@ -0,0 +1,6 @@ +image: maze.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 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 c02e6a0..44b6cdf 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 @@ -143,7 +143,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); } robot_nav_2d_msgs::Twist2D drive_target; - robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", transformed_plan.poses.front().pose.y, transformed_plan.poses.front().pose.theta); transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target); carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); diff --git a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp index eda7585..9b7b5bd 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp @@ -157,6 +157,15 @@ namespace robot_nav_core_adapter robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str()); throw std::runtime_error("Failed to load planner " + planner_name); } + 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__); + } new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_); planner_ = new_planner; planner_loader_ = new_loader;