diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 14cebc4..7764b05 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -1,11 +1,3 @@ -# position_planner_name: PNKXLocalPlanner -position_planner_name: HybridLocalPlanner -docking_planner_name: PNKXDockingLocalPlanner -go_straight_planner_name: PNKXGoStraightLocalPlanner -rotate_planner_name: PNKXRotateLocalPlanner -base_local_planner: LocalPlannerAdapter -base_global_planner: CustomPlanner - robot_base_frame: base_link transform_tolerance: 1.0 obstacle_range: 3.0 diff --git a/config/dock_global_params.yaml b/config/dock_global_params.yaml new file mode 100644 index 0000000..3191200 --- /dev/null +++ b/config/dock_global_params.yaml @@ -0,0 +1,11 @@ +DockPlanner: + library_path: libdock_planner + MyGlobalPlanner: + cost_threshold: 200 # Ngưỡng chi phí vật cản (0-255) + safety_distance: 2 # Khoảng cách an toàn (cells) + use_dijkstra: false # Sử dụng Dijkstra thay vì A* + + # File: config/costmap_params.yaml + global_costmap: + inflation_radius: 0.3 # Bán kính phình vật cản + cost_scaling_factor: 10.0 # Hệ số tỷ lệ chi phí \ No newline at end of file diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index 73110da..190579c 100755 --- a/config/move_base_common_params.yaml +++ b/config/move_base_common_params.yaml @@ -1,3 +1,25 @@ +position_planner_name: PNKXLocalPlanner +docking_planner_name: PNKXDockingLocalPlanner +go_straight_planner_name: PNKXGoStraightLocalPlanner +rotate_planner_name: PNKXRotateLocalPlanner +base_local_planner: LocalPlannerAdapter +base_global_planner: CustomPlanner + +PNKXLocalPlanner: + base_local_planner: LocalPlannerAdapter + base_global_planner: CustomPlanner + +PNKXDockingLocalPlanner: + base_local_planner: LocalPlannerAdapter + base_global_planner: TwoPointsPlanner + +PNKXGoStraightLocalPlanner: + base_local_planner: LocalPlannerAdapter + base_global_planner: TwoPointsPlanner + +PNKXRotateLocalPlanner: + base_local_planner: LocalPlannerAdapter + base_global_planner: TwoPointsPlanner ### replanning controller_frequency: 30.0 # run controller at 15.0 Hz diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 9b3aa9d..581f117 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -41,8 +41,8 @@ PNKXRotateLocalPlanner: LimitedAccelGenerator: library_path: libmkt_plugins_standard_traj_generator - max_vel_x: 0.2 - min_vel_x: -0.2 + max_vel_x: 1.2 + min_vel_x: -1.2 max_vel_y: 0.0 # diff drive robot min_vel_y: 0.0 # diff drive robot @@ -50,7 +50,7 @@ LimitedAccelGenerator: max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability min_speed_xy: 0.25 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity - max_vel_theta: 0.7 # max_rot_vel: 1.0 # choose slightly less than the base's capability + max_vel_theta: 0.4 # max_rot_vel: 1.0 # choose slightly less than the base's capability min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity acc_lim_x: 1.5 @@ -79,6 +79,14 @@ MKTAlgorithmDiffPredictiveTrajectory: index_samples: 60 follow_step_path: true + # Kalman filter tuning (filters v and w commands) + kf_q_v: 0.25 + kf_q_w: 0.8 + kf_r_v: 0.05 + kf_r_w: 0.08 + kf_p0: 0.5 + kf_filter_angular: false + # Lookahead use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) # only when false: diff --git a/examples/NavigationExample/NavigationAPI.cs b/examples/NavigationExample/NavigationAPI.cs index 1becdc2..146fbf1 100644 --- a/examples/NavigationExample/NavigationAPI.cs +++ b/examples/NavigationExample/NavigationAPI.cs @@ -9,7 +9,7 @@ namespace NavigationExample /// public class NavigationAPI { - private const string DllName = "libnav_c_api.so"; // Linux + private const string DllName = "/usr/local/lib/libnav_c_api.so"; // Linux // For Windows: "nav_c_api.dll" // For macOS: "libnav_c_api.dylib" @@ -152,6 +152,10 @@ namespace NavigationExample [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, string marker, PoseStamped goal); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_dock_to_nodes_edges(NavigationHandle handle, string marker, Node[] nodes, Edge[] edges, PoseStamped goal); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance); diff --git a/examples/NavigationExample/NavigationExample.csproj b/examples/NavigationExample/NavigationExample.csproj index 9ec6097..c98bcda 100644 --- a/examples/NavigationExample/NavigationExample.csproj +++ b/examples/NavigationExample/NavigationExample.csproj @@ -1,7 +1,7 @@ Exe - net10.0 + net6.0 true diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index a483249..89ca9fd 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -396,8 +396,8 @@ namespace NavigationExample PoseStamped goal = new PoseStamped(); goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId)); - goal.pose.position.x = 1.0; - goal.pose.position.y = 1.0; + goal.pose.position.x = 0.01; + goal.pose.position.y = 0.01; goal.pose.position.z = 0.0; goal.pose.orientation.x = 0.0; goal.pose.orientation.y = 0.0; @@ -406,9 +406,9 @@ namespace NavigationExample // Console.WriteLine("Docking to docking_point"); - // NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal); + NavigationAPI.navigation_dock_to(navHandle, "charger", goal); - NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal); + // NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal); // NavigationAPI.navigation_move_to_order(navHandle, order, goal); NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0); diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index d225bf1..626ec96 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/examples/run_example.sh b/examples/run_example.sh index 19bf39a..0a9d3be 100755 --- a/examples/run_example.sh +++ b/examples/run_example.sh @@ -27,6 +27,7 @@ echo "Building C API library..." cd "$BUILD_DIR" cmake .. make +sudo make install echo "Library built successfully!" @@ -83,9 +84,9 @@ fi # Luôn copy source code mới nhất (cập nhật file nếu đã có) cd "$EXAMPLE_DIR/NavigationExample" -# Bước 3: Copy library -echo "Copying library..." -cp "$LIB_DIR/libnav_c_api.so" . +# # Bước 3: Copy library +# echo "Copying library..." +# cp "$LIB_DIR/libnav_c_api.so" . # Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies # Main build directory (contains libtf3.so, librobot_cpp.so, etc.) diff --git a/src/Algorithms/Libraries/kalman/src/kalman.cpp b/src/Algorithms/Libraries/kalman/src/kalman.cpp index 4c44312..5e47d9f 100755 --- a/src/Algorithms/Libraries/kalman/src/kalman.cpp +++ b/src/Algorithms/Libraries/kalman/src/kalman.cpp @@ -55,11 +55,34 @@ void KalmanFilter::update(const Eigen::VectorXd& y) { if(!initialized) throw std::runtime_error("Filter is not initialized!"); + if (y.size() != m) + throw std::runtime_error("KalmanFilter::update(): measurement vector has wrong size"); + + if (A.rows() != n || A.cols() != n || C.rows() != m || C.cols() != n || + Q.rows() != n || Q.cols() != n || R.rows() != m || R.cols() != m || + P.rows() != n || P.cols() != n) + throw std::runtime_error("KalmanFilter::update(): matrix dimensions are inconsistent"); + x_hat_new = A * x_hat; P = A*P*A.transpose() + Q; - K = P*C.transpose()*(C*P*C.transpose() + R).inverse(); + + const Eigen::MatrixXd S = C * P * C.transpose() + R; + Eigen::LDLT ldlt(S); + if (ldlt.info() != Eigen::Success) + throw std::runtime_error("KalmanFilter::update(): failed to decompose innovation covariance"); + + // K = P*C' * inv(S) -> solve(S * X = (P*C')^T) + const Eigen::MatrixXd PCt = P * C.transpose(); + const Eigen::MatrixXd KT = ldlt.solve(PCt.transpose()); + if (ldlt.info() != Eigen::Success) + throw std::runtime_error("KalmanFilter::update(): failed to solve for Kalman gain"); + K = KT.transpose(); + x_hat_new += K * (y - C*x_hat_new); - P = (I - K*C)*P; + + // Joseph form: keeps P symmetric / PSD under numeric errors + const Eigen::MatrixXd IKC = I - K * C; + P = IKC * P * IKC.transpose() + K * R * K.transpose(); x_hat = x_hat_new; t += dt; diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index d7d2427..6a21147 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -412,6 +412,14 @@ namespace mkt_algorithm Eigen::MatrixXd Q; Eigen::MatrixXd R; Eigen::MatrixXd P; + + // Kalman filter tuning (for v and w filtering) + double kf_q_v_; + double kf_q_w_; + double kf_r_v_; + double kf_r_w_; + double kf_p0_; + bool kf_filter_angular_; #ifdef BUILD_WITH_ROS ros::Publisher lookahead_point_pub_; #endif diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 9b2e510..5a12570 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -35,6 +35,12 @@ mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory() cost_scaling_gain_(0.0), control_duration_(0.0), kf_(nullptr), + kf_q_v_(0.25), + kf_q_w_(0.8), + kf_r_v_(0.05), + kf_r_w_(0.08), + kf_p0_(0.5), + kf_filter_angular_(false), m_(0), n_(0) { @@ -83,16 +89,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( // kalman last_actuator_update_ = robot::Time::now(); - n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] - m_ = 2; // measurements: x, y, theta + // State: [v, a, j, w, alpha, jw] where: + // - v: linear velocity, a: linear accel, j: linear jerk + // - w: angular velocity, alpha: angular accel, jw: angular jerk + // Measurement: [v, w] + n_ = 6; + m_ = 2; double dt = control_duration_; // Khởi tạo ma trận A = Eigen::MatrixXd::Identity(n_, n_); C = Eigen::MatrixXd::Zero(m_, n_); Q = Eigen::MatrixXd::Zero(n_, n_); - R = Eigen::MatrixXd::Identity(m_, m_); - P = Eigen::MatrixXd::Identity(n_, n_); + R = Eigen::MatrixXd::Zero(m_, m_); + P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_); for (int i = 0; i < n_; i += 3) { @@ -103,15 +113,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( C(0, 0) = 1; C(1, 3) = 1; - Q(2, 2) = 0.1; - Q(5, 5) = 0.6; + Q(2, 2) = std::max(1e-12, kf_q_v_); + Q(5, 5) = std::max(1e-12, kf_q_w_); - R(0, 0) = 0.1; - R(1, 1) = 0.2; - - P(3, 3) = 0.4; - P(4, 4) = 0.4; - P(5, 5) = 0.4; + R(0, 0) = std::max(1e-12, kf_r_v_); + R(1, 1) = std::max(1e-12, kf_r_w_); kf_ = boost::make_shared(dt, A, C, Q, R, P); Eigen::VectorXd x0(n_); @@ -177,6 +183,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); use_cost_regulated_linear_velocity_scaling_ = false; } + + // Kalman filter tuning (filtering v and w commands) + nh_priv_.param("kf_q_v", kf_q_v_, kf_q_v_); + nh_priv_.param("kf_q_w", kf_q_w_, kf_q_w_); + nh_priv_.param("kf_r_v", kf_r_v_, kf_r_v_); + nh_priv_.param("kf_r_w", kf_r_w_, kf_r_w_); + nh_priv_.param("kf_p0", kf_p0_, kf_p0_); + nh_priv_.param("kf_filter_angular", kf_filter_angular_, kf_filter_angular_); + double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); control_duration_ = 1.0 / control_frequency; @@ -558,25 +573,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( result.velocity = drive_cmd; return result; } - - // Eigen::VectorXd y(2); - // y << drive_cmd.x, drive_cmd.theta; - - // // Cập nhật lại A nếu dt thay đổi - // for (int i = 0; i < n_; ++i) - // for (int j = 0; j < n_; ++j) - // A(i, j) = (i == j ? 1.0 : 0.0); - // for (int i = 0; i < n_; i += 3) - // { - // A(i, i + 1) = dt; - // A(i, i + 2) = 0.5 * dt * dt; - // A(i + 1, i + 2) = dt; - // } - // kf_->update(y, dt, A); - // double v_min = min_approach_linear_velocity_; - // drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); - // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x); - // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); } result.poses.clear(); result.poses.reserve(transformed_plan.poses.size()); @@ -606,7 +602,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( { // 1) Curvature from pure pursuit const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; - const double L2_min = 0.05; // m^2, chỉnh theo nhu cầu (0.02–0.1) + const double L2_min = 0.01; // m^2, chỉnh theo nhu cầu (0.02–0.1) const double L2_safe = std::max(L2, L2_min); const double L = std::sqrt(L2_safe); @@ -635,7 +631,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( v_target = std::copysign(min_approach_linear_velocity, sign_x); // 5) Angular speed from curvature - double w_target = v_target * kappa; + double w_target = v_target * kappa + std::copysign(carrot_pose.pose.theta * dt, kappa); if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) { if (trajectory.poses.size() >= 2) { @@ -687,6 +683,27 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( drive_cmd.x = velocity.x + dv; drive_cmd.theta = velocity.theta + dw; + + + Eigen::VectorXd y(2); + y << drive_cmd.x, drive_cmd.theta; + + // Cập nhật lại A nếu dt thay đổi + for (int i = 0; i < n_; ++i) + for (int j = 0; j < n_; ++j) + A(i, j) = (i == j ? 1.0 : 0.0); + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + kf_->update(y, dt, A); + double v_min = min_approach_linear_velocity_; + drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target)); + drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x); + if (kf_filter_angular_) + drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( @@ -772,16 +789,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( // (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate; bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate; -#ifdef BUILD_WITH_ROS - if (result) - ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); +// #ifdef BUILD_WITH_ROS + // if (result) + // ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); // else if(fabs(velocity.x) < min_speed_xy_) // { // ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta); // ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); // } -#endif +// #endif return result; } diff --git a/src/Algorithms/Packages/global_planners/dock_planner b/src/Algorithms/Packages/global_planners/dock_planner index da82431..d51ecc0 160000 --- a/src/Algorithms/Packages/global_planners/dock_planner +++ b/src/Algorithms/Packages/global_planners/dock_planner @@ -1 +1 @@ -Subproject commit da82431cd9fdee1aef407579357af9e573460389 +Subproject commit d51ecc0986a5ebe3ed728477bd2fa0aabbba85da diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index eac6f13..0953730 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -36,8 +36,6 @@ namespace two_points_planner if (!initialized_) { robot::NodeHandle nh_priv_("~/" + name); - robot::log_info("TwoPointsPlanner: Name is %s", name.c_str()); - int lethal_obstacle; nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20); lethal_obstacle_ = (unsigned char)lethal_obstacle; @@ -121,6 +119,7 @@ namespace two_points_planner robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__); return false; } + robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start); robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal); robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", @@ -148,21 +147,21 @@ namespace two_points_planner } plan.clear(); - plan.push_back(start); + // plan.push_back(start); - unsigned int mx_start, my_start; - unsigned int mx_end, my_end; - if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x, - start.pose.position.y, - mx_start, my_start) + // unsigned int mx_start, my_start; + // unsigned int mx_end, my_end; + // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x, + // start.pose.position.y, + // mx_start, my_start) - || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x, - goal.pose.position.y, - mx_end, my_end)) - { - robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__); - return false; - } + // || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x, + // goal.pose.position.y, + // mx_end, my_end)) + // { + // robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__); + // return false; + // } // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start)); // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end)); // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index 07ef9b6..51e318f 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -74,7 +74,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & if (!traj_generator_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator"); } robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name); traj_generator_->initialize(nh_traj_gen); @@ -83,7 +83,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator"); } std::string algorithm_nav_name; @@ -99,14 +99,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & if (!nav_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create algorithm"); } nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_); } catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create algorithm"); } std::string algorithm_rotate_name; @@ -121,7 +121,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & if (!rotate_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm"); } rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str()); @@ -129,7 +129,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm"); } std::string goal_checker_name; @@ -145,7 +145,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & if (!goal_checker_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create goal checker"); } goal_checker_->initialize(parent_); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str()); @@ -153,7 +153,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create goal checker"); } this->initializeOthers(); @@ -250,11 +250,16 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset() if(rotate_algorithm_) rotate_algorithm_->reset(); ret_nav_ = ret_angle_ = false; + robot::log_info_at(__FILE__, __LINE__, "Debug"); + parent_.printParams(); std::string algorithm_nav_name; planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + // parent_.setParam(algorithm_nav_name, original_papams_); robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name); - parent_.setParam(algorithm_nav_name, original_papams_); nh_algorithm.setParam("allow_rotate", false); + + robot::log_info_at(__FILE__, __LINE__, "Debug ở đây"); + parent_.printParams(); } void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) @@ -567,17 +572,50 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_ } pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name) - : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false), - is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("") + : initialized_(false), + is_detected_(false), + is_goal_reached_(false), + following_(false), + allow_rotate_(false), + xy_goal_tolerance_(0.05), + yaw_goal_tolerance_(0.05), + min_lookahead_dist_(0.4), + max_lookahead_dist_(1.0), + lookahead_time_(1.5), + angle_threshold_(0.4), + name_(name), + docking_planner_name_(), + docking_nav_name_(), + docking_planner_(nullptr), + docking_nav_(nullptr), + linear_(), + angular_(), + nh_(), + nh_priv_(), + tf_(nullptr), + costmap_robot_(nullptr), + traj_generator_(), + docking_planner_loader_(), + docking_nav_loader_(), + detected_timeout_wt_(), + delayed_wt_(), + delayed_(false), + detected_timeout_(false), + robot_base_frame_(), + maker_goal_frame_() { } pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner() { - if (docking_planner_) + detected_timeout_wt_.stop(); + delayed_wt_.stop(); + if (docking_planner_ != nullptr) { docking_planner_.reset(); - if (docking_nav_) + } + if (docking_nav_ != nullptr) { docking_nav_.reset(); + } } void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, @@ -593,14 +631,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob { try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(docking_planner_name_); docking_planner_loader_ = boost::dll::import_alias( path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations); docking_planner_ = docking_planner_loader_(); if (!docking_planner_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create global planner \"%s\": returned null pointer", docking_planner_name_.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create global planner"); } docking_planner_->initialize(name_, costmap_robot_); robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str()); @@ -616,14 +655,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob { try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(docking_nav_name_); docking_nav_loader_ = boost::dll::import_alias( path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations); docking_nav_ = docking_nav_loader_(); if (!docking_nav_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create docking nav"); } robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_); docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_); 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 ddbc668..4e11876 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 @@ -80,7 +80,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl if (!traj_generator_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator"); } robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name); traj_generator_->initialize(nh_traj_gen); @@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl catch (const std::exception& ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator"); } std::string algorithm_nav_name = std::string("pnkx_local_planner::GoStraight"); @@ -104,14 +104,14 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl if (!nav_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create algorithm"); } nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_); } catch (const std::exception& ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create algorithm"); } std::string goal_checker_name; @@ -126,7 +126,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl if (!goal_checker_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create goal checker"); } robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name); goal_checker_->initialize(nh_goal_checker); @@ -134,7 +134,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl catch (const std::exception& ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create goal checker"); } robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str()); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index 5989001..753985c 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, if (!traj_generator_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator"); } robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name); traj_generator_->initialize(nh_traj_gen); @@ -97,7 +97,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator"); } std::string algorithm_nav_name; @@ -107,20 +107,21 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, { robot::PluginLoaderHelper loader; std::string path_file_so = loader.findLibraryPath(algorithm_nav_name); + robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str()); nav_algorithm_loader_ = boost::dll::import_alias( path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations); nav_algorithm_ = nav_algorithm_loader_(); if (!nav_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create algorithm"); } nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_); } catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create algorithm"); } std::string algorithm_rotate_name; @@ -135,7 +136,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, if (!rotate_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm"); } rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str()); @@ -143,7 +144,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm"); } std::string goal_checker_name; @@ -159,7 +160,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, if (!goal_checker_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create goal checker"); } goal_checker_->initialize(parent_); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str()); @@ -167,7 +168,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create goal checker"); } this->initializeOthers(); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp index 0d4c5a5..f0b5718 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp @@ -64,7 +64,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p if (!traj_generator_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator"); } robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name); traj_generator_->initialize(nh_traj_gen); @@ -72,7 +72,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p catch (const std::exception& ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator"); } std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate"); @@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p if (!nav_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create algorithm"); } robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name); nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); @@ -96,7 +96,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p catch (const std::exception& ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create algorithm"); } std::string goal_checker_name; @@ -111,7 +111,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p if (!goal_checker_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create goal checker"); } robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name); goal_checker_->initialize(nh_goal_checker); @@ -119,7 +119,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p catch (const std::exception& ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what()); - exit(1); + throw robot_nav_core2::LocalPlannerException("Failed to create goal checker"); } robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str()); diff --git a/src/Libraries/robot_cpp/src/node_handle.cpp b/src/Libraries/robot_cpp/src/node_handle.cpp index 7d38393..2b70a79 100644 --- a/src/Libraries/robot_cpp/src/node_handle.cpp +++ b/src/Libraries/robot_cpp/src/node_handle.cpp @@ -651,7 +651,8 @@ namespace robot std::string key = it->first.as(); const YAML::Node &value = it->second; std::string full_key = prefix.empty() ? key : prefix + "/" + key; - + if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos) + continue; if (value.IsMap()) { std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl; diff --git a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp index bcdbdc9..ea1dbe8 100644 --- a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp +++ b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp @@ -66,6 +66,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name) } // Try to read from NodeHandle std::string library_path; + robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str()); if (nh_.hasParam(param_path)) { nh_.getParam(param_path, library_path, std::string("")); if (!library_path.empty()) { @@ -338,7 +339,7 @@ std::string PluginLoaderHelper::getBuildDirectory() std::string PluginLoaderHelper::getWorkspacePath() { // Method 1: Từ environment variable PNKX_NAV_CORE_DIR - const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR"); + const char* workspace_path = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH"); if (workspace_path && std::filesystem::exists(workspace_path)) { return std::string(workspace_path); } diff --git a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h index 5373293..09dfc71 100755 --- a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h +++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace robot_nav_core @@ -148,6 +149,12 @@ namespace robot_nav_core */ virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0; + /** + * @brief Set the odometry of the robot + * @param odom The odometry of the robot + */ + virtual void setOdom(robot_nav_msgs::Odometry *odom) { odom_ = odom; } + /** * @brief Virtual destructor for the interface */ @@ -155,6 +162,11 @@ namespace robot_nav_core protected: BaseLocalPlanner() {} + + /** + * @brief The odometry of the robot + */ + robot_nav_msgs::Odometry *odom_; }; } // namespace robot_nav_core diff --git a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h index 0bb1184..8bd4e12 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h +++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h @@ -189,11 +189,6 @@ namespace robot_nav_core_adapter robot_nav_2d_msgs::Pose2DStamped last_goal_; bool has_active_goal_; - /** - * @brief The odometry of the robot - */ - robot_nav_msgs::Odometry odom_; - // Plugin handling boost::function planner_loader_; robot_nav_core2::LocalPlanner::Ptr planner_; 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 3126c0e..e3b82e9 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 @@ -88,7 +88,7 @@ namespace robot_nav_core_adapter private_nh_ = robot::NodeHandle("~"); adapter_nh_ = robot::NodeHandle(private_nh_, name); - + std::string planner_name; if (adapter_nh_.hasParam("planner_name")) { @@ -291,7 +291,7 @@ namespace robot_nav_core_adapter robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist() { - return odom_.twist.twist; + return odom_->twist.twist; } bool LocalPlannerAdapter::islock() @@ -356,7 +356,7 @@ namespace robot_nav_core_adapter if (!getRobotPose(pose2d)) return false; - robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist); + robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist); bool ret = planner_->isGoalReached(pose2d, velocity); if (ret) { diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index b67434c..2c0185c 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -31,7 +31,9 @@ #include #include #include -#include +#ifndef BUILD_WITH_ROS + #include +#endif move_base::MoveBase::MoveBase() : initialized_(false), @@ -322,6 +324,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) throw std::runtime_error("Failed to load local planner " + local_planner); } tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_); + tc_->setOdom(&odometry_); } catch (const std::exception &ex) { @@ -500,6 +503,7 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) { auto it = laser_scans_.find(laser_scan_name); +#ifndef BUILD_WITH_ROS laser_filter::LaserScanSOR sor; sor.setMeanK(10); // xét 10 điểm láng giềng gần nhất sor.setStddevMulThresh(1.0); // ngưỡng = mean + 1.0 * stddev @@ -512,6 +516,16 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot { it->second = laser_scan_filter; } +#else + if (it == laser_scans_.end()) + { + laser_scans_[laser_scan_name] = laser_scan; + } + else + { + it->second = laser_scan; + } +#endif // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec); // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str()); // robot::log_info("angle_min: %f", laser_scan.angle_min); @@ -534,8 +548,13 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot // } // robot::log_error("intensities: %s", intensities_str.str().c_str()); +#ifndef BUILD_WITH_ROS updateLocalCostmap(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateGlobalCostmap(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); +#else + updateLocalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); + updateGlobalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); +#endif } robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name) @@ -1225,6 +1244,47 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry } if (cancel_ctr_) cancel_ctr_ = false; + // Check if action server exists + if (!as_) + { + robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + lock.unlock(); + return false; + } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + // Generate unique goal ID using timestamp + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + robot::log_info("[MoveBase::moveTo] Goal stamp: %ld.%09ld", + action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); + + // Clear Order message since this is a non-Order moveTo call + { + boost::unique_lock planner_lock(planner_mutex_); + planner_order_.reset(); + } + + robot::log_info("[MoveBase::moveTo] Processing goal through action server..."); + as_->processGoal(action_goal); + robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); + } + catch (const std::exception &e) + { + lock.unlock(); + robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what()); + return false; + } lock.unlock(); return true; @@ -1722,32 +1782,6 @@ void move_base::MoveBase::cancel() boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); cancel_ctr_ = true; robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true"); - - if (as_) - { - // Get current goal ID from action server to cancel specific goal - // If we want to cancel all goals, use empty string "" - robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared(); - - // Use empty string to cancel current goal (processCancel accepts empty string to cancel all) - msg->id = ""; // Empty string cancels current goal - msg->stamp = robot::Time::now(); - - robot::log_info("[MoveBase::cancel] Sending cancel request to action server"); - robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld", - msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec); - - // Convert to ConstPtr for processCancel - robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg; - as_->processCancel(cancel_msg); - robot::log_info("[MoveBase::cancel] Cancel request processed by action server"); - } - else - { - robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request"); - } - - robot::log_info("[MoveBase::cancel] ===== EXIT ====="); } bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose) @@ -2322,8 +2356,9 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y); } } - + robot::log_warning("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y); robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose); + robot::log_warning("Received goalToGlobalFrame: x=%.2f, y=%.2f", goal.pose.position.x, goal.pose.position.y); publishZeroVelocity(); // we have a goal so start the planner boost::unique_lock lock(planner_mutex_); @@ -2543,7 +2578,6 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio robot::log_error("Quaternion has nans or infs... discarding as a navigation goal"); return false; } - tf3::Quaternion tf_q(q.x, q.y, q.z, q.w); // next, we need to check if the length of the quaternion is close to zero @@ -2705,6 +2739,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) recovery_index_ = 0; } + // Cancle executeCycle if (cancel_ctr_ && tc_) { @@ -2713,13 +2748,25 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) tc_->setTwistLinear(linear); try { - if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_) + double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2)); + if (actual_linear_velocity <= min_approach_linear_velocity_) { robot::log_info("MoveTo is canceled."); resetState(); + cancel_ctr_ = false; // if(default_config_.base_global_planner != last_config_.base_global_planner) - swapPlanner(default_config_.base_global_planner); - + robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_); + if(nh_planner.hasParam("base_global_planner")) + { + std::string base_global_planner; + nh_planner.getParam("base_global_planner", base_global_planner); + swapPlanner(base_global_planner); + } + else + { + swapPlanner(default_config_.base_global_planner); + } + cancel_ctr_ = false; // disable the planner thread boost::unique_lock lock(planner_mutex_); runPlanner_ = false; @@ -2730,7 +2777,21 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED; } // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled."); - cancel_ctr_ = false; + + if (as_) + { + robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared(); + msg->id = ""; // Empty string cancels current goal + msg->stamp = robot::Time::now(); + // Convert to ConstPtr for processCancel + robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg; + as_->processCancel(cancel_msg); + robot::log_info("[MoveBase::cancel] Cancel request processed by action server"); + } + else + { + robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request"); + } return true; } } @@ -2739,7 +2800,17 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) robot::log_info("MoveTo is canceled."); resetState(); // if(default_config_.base_global_planner != last_config_.base_global_planner) - swapPlanner(default_config_.base_global_planner); + robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_); + if(nh_planner.hasParam("base_global_planner")) + { + std::string base_global_planner; + nh_planner.getParam("base_global_planner", base_global_planner); + swapPlanner(base_global_planner); + } + else + { + swapPlanner(default_config_.base_global_planner); + } // disable the planner thread boost::unique_lock lock(planner_mutex_); @@ -2751,6 +2822,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED; } // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled."); + if (as_) + { + robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared(); + msg->id = ""; // Empty string cancels current goal + msg->stamp = robot::Time::now(); + // Convert to ConstPtr for processCancel + robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg; + as_->processCancel(cancel_msg); + robot::log_info("[MoveBase::cancel] Cancel request processed by action server"); + } + else + { + robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request"); + } cancel_ctr_ = false; return true; } @@ -2771,7 +2856,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) // if we're controlling, we'll attempt to find valid velocity commands case move_base::CONTROLLING: - // robot::log_debug("In controlling state."); // check to see if we've reached our goal try @@ -2945,7 +3029,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) tc_->setTwistLinear(linear); try { - if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_) + double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2)); + if (actual_linear_velocity <= min_approach_linear_velocity_) { paused_ = true; } @@ -3049,15 +3134,48 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg) { - std::string global_frame = planner_costmap_robot_->getGlobalFrameID(); - robot_geometry_msgs::PoseStamped goal_pose, global_pose; - goal_pose = goal_pose_msg; - goal_pose.header.stamp = robot::Time(); // latest available + if (!tf_) + { + return goal_pose_msg; + } + + std::string global_frame = planner_costmap_robot_->getGlobalFrameID(); + // robot_geometry_msgs::PoseStamped goal_pose, global_pose; + // goal_pose = goal_pose_msg; + + // goal_pose.header.stamp = robot::Time(); // latest available + // try + // { + // tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time()); + // tf3::doTransform(goal_pose, global_pose, transform); + // } + robot_geometry_msgs::PoseStamped global_pose; + tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose); + robot_geometry_msgs::PoseStamped goal_pose; + tf3::toMsg(tf3::Transform::getIdentity(), goal_pose.pose); + goal_pose = goal_pose_msg; + robot::Time current_time = robot::Time::now(); // save time for checking tf delay later + tf3::Time tf3_current_time = data_convert::convertTime(current_time); + tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); + try { - tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time()); - tf3::doTransform(goal_pose, global_pose, transform); + // use current time if possible (makes sure it's not in the future) + std::string error_msg; + if (tf_->canTransform(global_frame, goal_pose.header.frame_id, tf3_current_time, &error_msg)) + { + // Transform is available at current time + tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_current_time); + tf3::doTransform(goal_pose, global_pose, transform); + } + // use the latest otherwise (tf3::Time() means latest available) + else + { + // Try to get latest transform + tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_zero_time); + tf3::doTransform(goal_pose, global_pose, transform); + } } catch (tf3::LookupException &ex) {