diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 5619b75..a847d64 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -1,3 +1,10 @@ +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 + robot_base_frame: base_footprint transform_tolerance: 1.0 obstacle_range: 3.0 diff --git a/config/custom_global_params.yaml b/config/custom_global_params.yaml index 71565e3..f462e6b 100644 --- a/config/custom_global_params.yaml +++ b/config/custom_global_params.yaml @@ -1,4 +1,3 @@ -base_global_planner: CustomPlanner CustomPlanner: library_path: libcustom_planner environment_type: XYThetaLattice diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 40dc56d..3667330 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -1,10 +1,4 @@ -position_planner_name: PNKXLocalPlanner -docking_planner_name: PNKXDockingLocalPlanner -go_straight_planner_name: PNKXGoStraightLocalPlanner -rotate_planner_name: PNKXRotateLocalPlanner - -base_local_planner: LocalPlannerAdapter LocalPlannerAdapter: library_path: liblocal_planner_adapter yaw_goal_tolerance: 0.017 @@ -92,7 +86,7 @@ MKTAlgorithmDiffPredictiveTrajectory: # only when false: lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) # only when true: - min_lookahead_dist: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3) + min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3) max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9) lookahead_time: 1.6 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) @@ -120,6 +114,20 @@ MKTAlgorithmDiffPredictiveTrajectory: cost_scaling_dist: 0.2 # (default: 0.6) cost_scaling_gain: 2.0 # (default: 1.0) + + use_mpc: true + mpc_horizon: 10 + mpc_dt: 0.1 + mpc_w_pos: 6.0 + mpc_w_theta: 2.0 + mpc_w_v: 0.2 + mpc_w_w: 0.2 + mpc_w_dv: 0.4 + mpc_w_dw: 0.4 + mpc_iterations: 3 + mpc_step: 0.15 + mpc_eps: 0.001 + MKTAlgorithmDiffGoStraight: library_path: libmkt_algorithm_diff avoid_obstacles: false @@ -133,7 +141,7 @@ MKTAlgorithmDiffGoStraight: # only when false: lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) # only when true: - min_lookahead_dist: 0.8 # The minimum lookahead distance (m) threshold. (default: 0.3) + min_lookahead_dist: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3) max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) lookahead_time: 2.5 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) diff --git a/config/two_points_global_params.yaml b/config/two_points_global_params.yaml index 99f7480..cb9ca6b 100644 --- a/config/two_points_global_params.yaml +++ b/config/two_points_global_params.yaml @@ -1,4 +1,3 @@ -base_global_planner: TwoPointsPlanner TwoPointsPlanner: library_path: libtwo_points_planner lethal_obstacle: 20 \ No newline at end of file diff --git a/examples/CSharpExample.cs b/examples/CSharpExample.cs index 3d52a77..6ffb99a 100644 --- a/examples/CSharpExample.cs +++ b/examples/CSharpExample.cs @@ -53,6 +53,14 @@ namespace NavigationExample public double theta; } + [StructLayout(LayoutKind.Sequential)] + public struct Twist2D + { + public double x; + public double y; + public double theta; + } + [StructLayout(LayoutKind.Sequential)] public struct Quaternion { @@ -93,6 +101,13 @@ namespace NavigationExample public Pose pose; } + [StructLayout(LayoutKind.Sequential)] + public struct Twist2DStamped + { + public Header header; + public Twist2D velocity; + } + [StructLayout(LayoutKind.Sequential)] public struct Vector3 { @@ -180,6 +195,14 @@ namespace NavigationExample public static extern bool navigation_set_robot_footprint( IntPtr handle, Point[] points, UIntPtr point_count); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_footprint( + IntPtr handle, out IntPtr out_points, out UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_free_points(IntPtr points); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_move_to( @@ -231,6 +254,11 @@ namespace NavigationExample public static extern bool navigation_get_robot_pose_2d( IntPtr handle, ref Pose2D out_pose); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_twist( + IntPtr handle, ref Twist2DStamped out_twist); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_get_feedback( diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 3d52a77..6ffb99a 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -53,6 +53,14 @@ namespace NavigationExample public double theta; } + [StructLayout(LayoutKind.Sequential)] + public struct Twist2D + { + public double x; + public double y; + public double theta; + } + [StructLayout(LayoutKind.Sequential)] public struct Quaternion { @@ -93,6 +101,13 @@ namespace NavigationExample public Pose pose; } + [StructLayout(LayoutKind.Sequential)] + public struct Twist2DStamped + { + public Header header; + public Twist2D velocity; + } + [StructLayout(LayoutKind.Sequential)] public struct Vector3 { @@ -180,6 +195,14 @@ namespace NavigationExample public static extern bool navigation_set_robot_footprint( IntPtr handle, Point[] points, UIntPtr point_count); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_footprint( + IntPtr handle, out IntPtr out_points, out UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_free_points(IntPtr points); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_move_to( @@ -231,6 +254,11 @@ namespace NavigationExample public static extern bool navigation_get_robot_pose_2d( IntPtr handle, ref Pose2D out_pose); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_twist( + IntPtr handle, ref Twist2DStamped out_twist); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_get_feedback( diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 3ffa135..8c6aee6 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/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index fab4538..1be32a6 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/src/APIs/c_api/include/nav_c_api.h @@ -59,6 +59,15 @@ typedef struct { double theta; } Pose2D; +/** + * @brief Twist2D structure (x, y, theta velocities) + */ +typedef struct { + double x; + double y; + double theta; +} Twist2D; + /** * @brief Quaternion structure */ @@ -104,6 +113,14 @@ typedef struct { Pose pose; } PoseStamped; +/** + * @brief Twist2DStamped structure + */ +typedef struct { + Header header; + Twist2D velocity; +} Twist2DStamped; + /** * @brief Vector3 structure */ @@ -250,6 +267,21 @@ bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle); */ bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count); +/** + * @brief Get the robot's footprint (outline shape) + * @param handle Navigation handle + * @param out_points Output array of points (allocated by library, free with navigation_free_points) + * @param out_count Output number of points in the array + * @return true on success, false on failure + */ +bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count); + +/** + * @brief Free a points array allocated by navigation_get_robot_footprint + * @param points Pointer to point array + */ +void navigation_free_points(Point* points); + /** * @brief Send a goal for the robot to navigate to * @param handle Navigation handle @@ -350,6 +382,15 @@ bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out */ bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose); +/** + * @brief Get the robot's current twist + * @param handle Navigation handle + * @param out_twist Output parameter with the robot's current twist + * @return true if twist was successfully retrieved + * @note out_twist->header.frame_id must be freed using nav_c_api_free_string + */ +bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist); + /** * @brief Get navigation feedback * @param handle Navigation handle diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index bc53e30..7cd747d 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -6,12 +6,14 @@ #include #include #include +#include +#include #include #include #include #include #include -#include +#include #include @@ -65,6 +67,25 @@ namespace { c_pose->theta = cpp_pose.theta; } + void cpp_to_c_header(const robot_std_msgs::Header& cpp_header, Header* c_header) { + c_header->seq = cpp_header.seq; + c_header->sec = cpp_header.stamp.sec; + c_header->nsec = cpp_header.stamp.nsec; + if (!cpp_header.frame_id.empty()) { + c_header->frame_id = strdup(cpp_header.frame_id.c_str()); + } else { + c_header->frame_id = nullptr; + } + } + + void cpp_to_c_twist2d_stamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist, + Twist2DStamped* c_twist) { + cpp_to_c_header(cpp_twist.header, &c_twist->header); + c_twist->velocity.x = cpp_twist.velocity.x; + c_twist->velocity.y = cpp_twist.velocity.y; + c_twist->velocity.theta = cpp_twist.velocity.theta; + } + // Convert C++ NavFeedback to C NavFeedback void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) { c_feedback->navigation_state = static_cast(static_cast(cpp_feedback.navigation_state)); @@ -226,22 +247,13 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle } try { printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); - robot::move_base_core::BaseNavigation* nav = static_cast(handle); + auto* nav = static_cast(handle); auto* tf_ptr = static_cast*>(tf_handle); - robot::PluginLoaderHelper loader; - std::string lib_path = loader.findLibraryPath("MoveBase"); - auto create_plugin = boost::dll::import_alias( - lib_path, - "MoveBase", - boost::dll::load_mode::append_decorations); - - robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin(); - printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__); - if (nav_ptr == nullptr) { - printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__); + if (!tf_ptr || !(*tf_ptr)) { + printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); return false; } - nav_ptr->initialize(*tf_ptr); + nav->initialize(*tf_ptr); return true; } catch (const std::exception &e) { @@ -276,6 +288,45 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po } } +extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count) { + if (!handle || !out_points || !out_count) { + return false; + } + + try { + robot::move_base_core::BaseNavigation* nav = static_cast(handle); + std::vector footprint = nav->getRobotFootprint(); + if (footprint.empty()) { + *out_points = nullptr; + *out_count = 0; + return true; + } + + Point* points = static_cast(malloc(sizeof(Point) * footprint.size())); + if (!points) { + return false; + } + + for (size_t i = 0; i < footprint.size(); ++i) { + points[i].x = footprint[i].x; + points[i].y = footprint[i].y; + points[i].z = footprint[i].z; + } + + *out_points = points; + *out_count = footprint.size(); + return true; + } catch (...) { + return false; + } +} + +extern "C" void navigation_free_points(Point* points) { + if (points) { + free(points); + } +} + extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal, double xy_goal_tolerance, double yaw_goal_tolerance) { if (!handle || !goal) { @@ -442,6 +493,21 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou } } +extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist) { + if (!handle || !out_twist) { + return false; + } + + try { + robot::move_base_core::BaseNavigation* nav = static_cast(handle); + robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist(); + cpp_to_c_twist2d_stamped(cpp_twist, out_twist); + return true; + } catch (...) { + return false; + } +} + extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) { if (!handle || !out_feedback) { return false; diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index 1f54a64..792d5da 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -84,6 +84,7 @@ add_library(${PROJECT_NAME}_diff SHARED src/diff/diff_predictive_trajectory.cpp src/diff/diff_rotate_to_goal.cpp src/diff/diff_go_straight.cpp + # src/diff/pure_pursuit.cpp ) if(BUILDING_WITH_CATKIN) diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h new file mode 100644 index 0000000..95202fe --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h @@ -0,0 +1,50 @@ +#ifndef _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__ +#define _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mkt_algorithm +{ + namespace diff + { + class PurePursuit : public score_algorithm::ScoreAlgorithm + { + public: + PurePursuit(); + ~PurePursuit(); + void computePurePursuit( + const double &velocity_min, + const double &velocity_max, + const double &linear_vel, + const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + const double &min_dist2, + const double &curvature_dist2_floor, + const double &dist2_override); + + private: + void applyConstraints(const double &dist_error, const double &lookahead_dist, + const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, + const double &pose_cost, double &linear_vel, double &sign_x); + + double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + const double &journey_plan); + + double computeDecelerationFactor(const double &effective_journey, const double &d_reduce); + + bool detectWobbleByCarrotAngle(std::vector &angle_history, const double &carrot_angle, + const double &litude_threshold, const size_t &window_size); + }; + } +} + +#endif \ No newline at end of file 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 23f5c21..469cd5e 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 @@ -107,7 +107,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( robot_nav_2d_msgs::Twist2D twist; traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom robot::Rate r(50); - while (traj_->hasMoreTwists()) + while (robot::ok() && traj_->hasMoreTwists()) { twist = traj_->nextTwist(); // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta); @@ -184,6 +184,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( double v_min = journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; v_min *= sign_x; + double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); journey_plan += max_path_distance_; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp new file mode 100644 index 0000000..c5c68b4 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp @@ -0,0 +1,1327 @@ +#include +#include +#include + +#define LIMIT_VEL_THETA 0.33 + +void mkt_algorithm::diff::PredictiveTrajectory::initialize( + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) +{ + if (!initialized_) + { + nh_ = robot::NodeHandle("~"); + nh_priv_ = robot::NodeHandle(nh, name); + name_ = name; + tf_ = tf; + traj_ = traj; + costmap_robot_ = costmap_robot; + this->getParams(); + nh_.param("publish_topic", enable_publish_, false); + nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); + + footprint_spec_ = costmap_robot_->getRobotFootprint(); + std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const robot_geometry_msgs::Point &p1 = footprint[i]; + const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + this->initKalmanFilter(); + x_direction_ = y_direction_ = theta_direction_ = 0; + this->initialized_ = true; + robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); + } +} + +mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() +{ + if (kf_) + { + kf_.reset(); + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter() +{ + // kalman + last_actuator_update_ = robot::Time::now(); + n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] + m_ = 2; // measurements: x, y, theta + 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_); + + 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; + } + + C(0, 0) = 1; + C(1, 3) = 1; + Q(2, 2) = 0.1; + Q(5, 5) = 0.6; + + 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; + + kf_ = boost::make_shared(dt, A, C, Q, R, P); + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); +} + +void mkt_algorithm::diff::PredictiveTrajectory::getParams() +{ + robot_base_frame_ = nh_priv_.param("robot_base_frame", std::string("base_link")); + nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); + nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); + nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); + nh_priv_.param("index_samples", index_samples_, 0); + nh_priv_.param("follow_step_path", follow_step_path_, false); + + nh_priv_.param("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false); + nh_priv_.param("lookahead_dist", lookahead_dist_, 0.25); + nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.3); + nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 0.9); + nh_priv_.param("lookahead_time", lookahead_time_, 1.5); + nh_priv_.param("min_journey_squared", min_journey_squared_, std::numeric_limits::infinity()); + nh_priv_.param("max_journey_squared", max_journey_squared_, std::numeric_limits::infinity()); + + nh_priv_.param("use_rotate_to_heading", use_rotate_to_heading_, false); + nh_priv_.param("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785); + + nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); + nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); + if (trans_stopped_velocity_ <= min_approach_linear_velocity_ || trans_stopped_velocity_ - min_approach_linear_velocity_ < 0.02) + trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.05; + nh_priv_.param("cmd_smoothing_time", cmd_smoothing_time_, 0.2); + nh_priv_.param("max_lateral_accel", max_lateral_accel_, 0.6); + + // Regulated linear velocity scaling + nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); + nh_priv_.param("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9); + nh_priv_.param("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25); + + // Inflation cost scaling (Limit velocity by proximity to obstacles) + nh_priv_.param("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true); + nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); + nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); + nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); + if (inflation_cost_scaling_factor_ <= 0.0) + { + 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."); + use_cost_regulated_linear_velocity_scaling_ = false; + } + double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + control_duration_ = 1.0 / control_frequency; + window_size_ = (size_t)(control_frequency * 3.0); + + if (traj_) + { + traj_.get()->getNodeHandle().param("max_vel_x", max_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_x", min_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_x", acc_lim_x_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_x", decel_lim_x_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_y", max_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_y", min_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_y", acc_lim_y_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_y", decel_lim_y_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_theta", max_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::reset() +{ + if (this->initialized_) + { + robot::log_info("[%s:%d]\n PredictiveTrajectory is reset", __FILE__, __LINE__); + reached_intermediate_goals_.clear(); + start_index_saved_vt_.clear(); + this->clear(); + x_direction_ = y_direction_ = theta_direction_ = 0; + this->follow_step_path_ = false; + this->nav_stop_ = false; + last_actuator_update_ = robot::Time::now(); + prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); + + if (kf_) + { + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); + } + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::stop() +{ + this->nav_stop_ = true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::resume() +{ + if (this->initialized_) + { + if(!this->nav_stop_) + return; + prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); + last_actuator_update_ = robot::Time::now(); + + if (kf_) + { + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); + } + this->nav_stop_ = false; + } +} + +bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) +{ + if (!initialized_) + { + robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); + return false; + } + + std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const robot_geometry_msgs::Point &p1 = footprint[i]; + const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + + if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) + { + robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); + return false; + } + this->getParams(); + + frame_id_path_ = global_plan.header.frame_id; + goal_ = goal; + global_plan_ = global_plan; + + // prune global plan to cut off parts of the past (spatially before the robot) + if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0)) + { + robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__); + return false; + } + + double S = std::numeric_limits::infinity(); + S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0, + costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0); + const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_; + S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S); + compute_plan_.poses.clear(); + + if ((unsigned int)global_plan_.poses.size() == 2) + { + double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x; + double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y; + if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution()) + { + compute_plan_ = global_plan_; + } + else + { + robot::log_error("[%s:%d]\n The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution()); + return false; + } + } + else + { + unsigned int start_index, goal_index; + if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_)) + { + robot::log_error("[%s:%d]\n computePlanCommand is Failed", __FILE__, __LINE__); + return false; + } + } + + double lookahead_dist = this->getLookAheadDistance(velocity); + transform_plan_.poses.clear(); + if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_)) + { + robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); + return false; + } + // else + // { + // robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); + // } + + x_direction = x_direction_; + y_direction = y_direction_ = 0; + theta_direction = theta_direction_; + + if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq) + { + const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose; + int index; + for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--) + { + robot_geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose; + if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_) + break; + } + const robot_geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose; + if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x); + const double dir_path = cos(fabs(path_angle - p2.theta)); + if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) + x_direction = dir_path > 0 ? FORWARD : BACKWARD; + } + } + else + { + try + { + auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_); + auto prev_carrot_pose_it = transform_plan_.poses.begin(); + double distance_it = 0; + if (carrot_pose_it != transform_plan_.poses.begin()) + { + for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) + { + double dx = it->pose.x - carrot_pose_it->pose.x; + double dy = it->pose.y - carrot_pose_it->pose.y; + distance_it += std::hypot(dx, dy); + if (distance_it > costmap_robot_->getCostmap()->getResolution()) + { + prev_carrot_pose_it = it; + break; + } + } + } + + robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution() + ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) + : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D()); + + robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); + + // teb_local_planner::PoseSE2 start_pose(front); + // teb_local_planner::PoseSE2 goal_pose(back); + // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec()); + const double dir_path = 0.0; + + if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) + x_direction = dir_path > 0 ? FORWARD : BACKWARD; + } + catch (std::exception &e) + { + robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what()); + return false; + } + } + + x_direction_ = x_direction; + y_direction_ = y_direction; + theta_direction_ = theta_direction; + return true; +} + +mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) +{ + mkt_msgs::Trajectory2D result; + result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; + if (!traj_) + return result; + if (compute_plan_.poses.size() < 2) + { + robot::log_warning("[%s:%d]\n Local compute plan is available", __FILE__, __LINE__); + return result; + } + + robot::Time current_time = robot::Time::now(); + double dt = (current_time - last_actuator_update_).toSec(); + last_actuator_update_ = current_time; + + robot_nav_2d_msgs::Twist2D drive_cmd; + double sign_x = sign(x_direction_); + robot_nav_2d_msgs::Twist2D twist; + robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta); + traj_->startNewIteration(velocity); + while (robot::ok() && traj_->hasMoreTwists()) + { + twist = traj_->nextTwist(); + } + drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); + robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta); + robot::log_info("--------------------------------"); + double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + + robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; + result.poses.clear(); + result.poses.reserve(transformed_plan.poses.size()); + for (const auto &pose_stamped : transformed_plan.poses) + { + result.poses.push_back(pose_stamped.pose); + } + + if (transformed_plan.poses.empty()) + { + robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); + return result; + } + + double lookahead_dist = getLookAheadDistance(velocity); + double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y); + + if (transformed_plan.poses.empty()) + { + robot::log_warning("[%s:%d]\n Transformed plan is empty after compute lookahead point", __FILE__, __LINE__); + return result; + } + auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); + bool allow_rotate = false; + nh_priv_.param("allow_rotate", allow_rotate, false); + + // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y); + robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose; + const double distance_allow_rotate = min_journey_squared_; + const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y); + const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1); + if (avoid_obstacles_) + allow_rotate = journey_plan >= distance_allow_rotate && + fabs(front.y) <= 0.2 && + (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution()); + else + allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; + + double angle_to_heading; + if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) + { + if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + { + rotateToHeading(angle_to_heading, velocity, drive_cmd); + } + } + else + { + const double vel_x_reduce = std::min(fabs(v_max), 0.3); + double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; + carrot_dist2 = std::max(carrot_dist2, 0.05); + double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + + if (max_lateral_accel_ > 1e-6) + { + const double curvature_abs = std::max(fabs(curvature), 1e-6); + const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs); + drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit); + } + + const auto &plan_back_pose = compute_plan_.poses.back(); + double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y); + post_cost = std::max(post_cost, center_cost_); + this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); + + const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; + const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; + double d_reduce = std::clamp(journey_plan, min_S, max_S); + double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); + double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); + double v_min = + journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; + v_min *= sign_x; + + double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); + double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); + double vel_reduce = sign_x > 0 + ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) + : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); + drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; + + double v_theta = drive_cmd.x * curvature; + double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_)) + { + carrot_dist2 *= 0.6; + curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + v_theta = drive_cmd.x * curvature; + } + if (fabs(v_theta) > LIMIT_VEL_THETA) + { + robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result; + cmd_vel.x = sign_x > 0 + ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1)) + : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1)); + cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5); + this->moveWithAccLimits(velocity, cmd_vel, cmd_result); + drive_cmd.x = std::copysign(cmd_result.x, sign_x); + v_theta = drive_cmd.x * curvature; + } + + if (journey_plan < min_journey_squared_) + { + if (transform_plan_.poses.size() > 2) + { + robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); + robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; + double dx = end.pose.x - start.pose.x; + double dy = end.pose.y - start.pose.y; + v_theta = atan2(dy, dx); + if (v_theta > M_PI_2) + v_theta -= M_PI; + else if (v_theta < -M_PI_2) + v_theta += M_PI; + // v_theta *= 0.5; + v_theta = std::clamp(v_theta, -0.02, 0.02); + } + else + v_theta = 0.0; + } + double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8; + double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; + double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; + drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); + + if (this->nav_stop_) + { + if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + drive_cmd = {}; + 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); + + // Check if Kalman filter's estimated velocity exceeds v_max + if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || + cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || + cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE || + cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE)) + { + drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0]; + } + else + { + drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); + } + + drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x); + drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA); + } + + if (cmd_smoothing_time_ > 1e-6) + { + const double alpha = std::clamp(dt / (cmd_smoothing_time_ + dt), 0.0, 1.0); + drive_cmd.x = prevous_drive_cmd_.x + alpha * (drive_cmd.x - prevous_drive_cmd_.x); + drive_cmd.theta = prevous_drive_cmd_.theta + alpha * (drive_cmd.theta - prevous_drive_cmd_.theta); + } + + result.velocity = drive_cmd; + prevous_drive_cmd_ = drive_cmd; + return result; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( + const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x) +{ + bool curvature = false; + double path_angle = 0; + if (!global_plan.poses.empty()) + { + if ((unsigned)global_plan.poses.size() > 2) + { + const double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + const double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + { + double start_angle = atan2(start_direction_y, start_direction_x); + for (unsigned int i = 1; i < (unsigned)global_plan.poses.size(); i++) + { + const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; + const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; + if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + { + double current_angle = atan2(current_direction_y, current_direction_x); + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > 1e-9) + { + curvature = true; + break; + } + path_angle = current_angle; + } + } + } + } + } + // ROS_INFO_THROTTLE(0.1,"path_angle %.3f %.3f", path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x)); + // Whether we should rotate robot to rough path heading + angle_to_path = curvature ? path_angle : std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + angle_to_path = sign_x < 0 + ? angles::normalize_angle(M_PI + angle_to_path) + : angles::normalize_angle(angle_to_path); + + double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y); + // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + double heading_rotate = rotate_to_heading_min_angle_; + bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_); + heading_rotate = is_stopped + ? rotate_to_heading_min_angle_ + heading_linear + : std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2); + + bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate; + + // if (result) + // { + // ROS_WARN_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f", + // velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_); + // ROS_WARN_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n", + // is_stopped, path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x), angle_to_path, heading_rotate, sign_x); + // } + // else + // { + // ROS_INFO_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f", + // velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_); + // ROS_INFO_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n", + // is_stopped, path_angle,std::atan2(carrot_pose.pose.y, carrot_pose.pose.x) ,angle_to_path, heading_rotate, sign_x); + // } + + return result; +} + +void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) +{ + const double dt = control_duration_; + const double theta_direction = angle_to_path > 0.0 ? 1.0 : -1.0; + double vel_yaw = theta_direction * fabs(velocity.theta); + double ang_diff = angle_to_path; + double max_vel_theta = traj_ ? fabs(traj_->getTwistAngular(true).z) : 0.3; + + double angular_decel_zone = 0.3; // rad - khoảng cách bắt đầu giảm tốc + nh_priv_.param("angular_decel_zone", angular_decel_zone, 0.5); + angular_decel_zone += std::clamp( + fabs(max_vel_theta) + (M_PI_2 - fabs(atan2(min_path_distance_, max_path_distance_))), 0.4, M_PI_2); + double zone_begin = std::max(angular_decel_zone * 0.2, 0.2); + double cosine_begin_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / zone_begin))); + + double reduce_vel_theta = std::min(max_vel_theta, 0.25 + min_vel_theta_); + reduce_vel_theta = fabs(ang_diff) > zone_begin ? reduce_vel_theta : (reduce_vel_theta - min_vel_theta_) * cosine_begin_factor + min_vel_theta_; + + // === ANGULAR DECELERATION ZONE === + double target_angular_speed = max_vel_theta; + if (fabs(ang_diff) < angular_decel_zone) + { + // ROS_WARN_THROTTLE(0.2,"%f %f %f %f", ang_diff, angular_decel_zone, zone_begin, max_vel_theta); + double cosine_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / angular_decel_zone))); + double cosine_speed = max_vel_theta * cosine_factor; + + // Choose deceleration profile + target_angular_speed = cosine_speed; // Smoothest option + + // Ensure minimum speed to avoid stalling + target_angular_speed = std::max(target_angular_speed, reduce_vel_theta); + } + // else + // ROS_INFO_THROTTLE(1,"%f %f %f %f", ang_diff, angular_decel_zone, max_vel_theta, reduce_vel_theta); + + // Apply direction + double v_theta_desired = std::copysign(target_angular_speed, ang_diff); + + // === ACCELERATION LIMITS === + double max_acc_vel = vel_yaw + fabs(acc_lim_theta_) * dt; + double min_acc_vel = vel_yaw - fabs(acc_lim_theta_) * dt; + double v_theta_samp = std::clamp(v_theta_desired, min_acc_vel, max_acc_vel); + + // === STOPPING CONSTRAINT === + double max_speed_to_stop = sqrt(2.0 * fabs(decel_lim_theta_) * fabs(ang_diff)); + v_theta_samp = std::copysign( + std::min(max_speed_to_stop, fabs(v_theta_samp)), ang_diff); + + // === FINAL LIMITS === + v_theta_samp = theta_direction > 0 + ? std::clamp(v_theta_samp, min_vel_theta_, max_vel_theta) + : std::clamp(v_theta_samp, -max_vel_theta, -min_vel_theta_); + + cmd_vel.x = 0.0; + cmd_vel.y = 0.0; + cmd_vel.theta = v_theta_samp; +} + +double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) + { + lookahead_dist = fabs(velocity.x) * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +std::vector::iterator +mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan) +{ + if (global_plan.poses.empty()) + { + throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty."); + } + + if ((unsigned)global_plan.poses.size() < 2) + { + auto goal_pose_it = std::prev(global_plan.poses.end()); + return goal_pose_it; + } + unsigned int goal_index = (unsigned)global_plan.poses.size() - 1; + + double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; + double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; + + // make sure that atan2 is defined + double start_angle = atan2(start_direction_y, start_direction_x); + double end_angle = atan2(end_direction_y, end_direction_x); + double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); + + for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) + { + if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + { + const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; + const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; + + if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + { + double current_angle = atan2(current_direction_y, current_direction_x); + goal_index = i; + if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) + continue; + + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) + { + goal_index = i; + break; + } + } + } + } + + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + global_plan.poses.begin(), global_plan.poses.begin() + goal_index, + [&](const auto &ps) + { + return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; + }); + + // If the number of poses is not far enough, take the last pose + if (goal_pose_it == global_plan.poses.end()) + { + goal_pose_it = std::prev(global_plan.poses.end()); + } + return goal_pose_it; +} + +robot_geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose) +{ + robot_geometry_msgs::PointStamped carrot_msg; + carrot_msg.header = carrot_pose.header; + carrot_msg.point.x = carrot_pose.pose.x; + carrot_msg.point.y = carrot_pose.pose.y; + carrot_msg.point.z = 0.5; // publish right over map to stand out + return carrot_msg; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot) +{ + if (global_plan.poses.empty()) + return false; + if (tf == nullptr) + return false; + try + { + // let's get the pose of the robot in the frame of the plan + robot_nav_2d_msgs::Pose2DStamped robot; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) + { + throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame"); + } + + double dist_thresh_sq = dist_behind_robot * dist_behind_robot; + + // iterate plan until a pose close the robot is found + std::vector::iterator it = global_plan.poses.begin(); + std::vector::iterator erase_end = it; + while (it != global_plan.poses.end()) + { + double dx = robot.pose.x - it->pose.x; + double dy = robot.pose.y - it->pose.y; + double dist_sq = dx * dx + dy * dy; + if (dist_sq < dist_thresh_sq) + { + erase_end = it; + break; + } + ++it; + } + if (erase_end == global_plan.poses.end()) + return false; + + if (erase_end != global_plan.poses.begin()) + global_plan.poses.erase(global_plan.poses.begin(), erase_end); + } + catch (const tf3::TransformException &ex) + { + robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what()); + return false; + } + return true; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( + TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, + robot_nav_2d_msgs::Path2D &transformed_plan) +{ + // this method is a slightly modified version of base_local_planner/goal_functions.h + transformed_plan.poses.clear(); + transformed_plan.header.stamp = pose.header.stamp; + transformed_plan.header.frame_id = robot_base_frame; + + if (global_plan.poses.empty()) + return false; + + if (tf == nullptr) + return false; + + if (costmap == nullptr || costmap->getCostmap() == nullptr) + return false; + + try + { + if (global_plan.poses.empty()) + { + robot::log_error("[%s:%d]\n Received plan with zero length", __FILE__, __LINE__); + return false; + } + + // let's get the pose of the robot in the frame of the plan + robot_nav_2d_msgs::Pose2DStamped robot_pose; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) + { + throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame"); + } + + // we'll discard points on the plan that are outside the local costmap + double dist_threshold = std::max(costmap->getCostmap()->getSizeInCellsX() * costmap->getCostmap()->getResolution() / 2.0, + costmap->getCostmap()->getSizeInCellsY() * costmap->getCostmap()->getResolution() / 2.0); + + dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are + double sq_dist_threshold = dist_threshold * dist_threshold; + // located on the border of the local costmap + + int i = 0; + double sq_dist = std::numeric_limits::infinity(); + + // we need to loop to a point on the plan that is within a certain distance of the robot + bool robot_reached = false; + for (int j = 0; j < (int)global_plan.poses.size(); ++j) + { + double x_diff = robot_pose.pose.x - global_plan.poses[j].pose.x; + double y_diff = robot_pose.pose.y - global_plan.poses[j].pose.y; + double new_sq_dist = x_diff * x_diff + y_diff * y_diff; + if (robot_reached && new_sq_dist > sq_dist_threshold) + { + break; + } // force stop if we have reached the costmap border + + if (new_sq_dist < sq_dist) // find closest distance + { + sq_dist = new_sq_dist; + i = j; + if (sq_dist < costmap->getCostmap()->getResolution()) // 5 cm to the robot; take the immediate local minima; if it's not the global + robot_reached = true; // minima, probably means that there's a loop in the path, and so we prefer this + } + } + + double plan_length = 0; // check cumulative Euclidean distance along the plan + double x_direction_saved = 0.0; + // now we'll transform until points are outside of our distance threshold + while (i < (int)global_plan.poses.size() && sq_dist <= sq_dist_threshold && + (max_plan_length <= 0 || plan_length <= max_plan_length)) + { + robot_nav_2d_msgs::Pose2DStamped stamped_pose; + stamped_pose.pose = global_plan.poses[i].pose; + stamped_pose.header.frame_id = global_plan.header.frame_id; + + robot_nav_2d_msgs::Pose2DStamped newer_pose; + if (!robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, newer_pose)) + { + ++i; + continue; + } + transformed_plan.poses.push_back(newer_pose); + + double x_diff = robot_pose.pose.x - global_plan.poses[i].pose.x; + double y_diff = robot_pose.pose.y - global_plan.poses[i].pose.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + // Calculate distance to previous pose + if (i > 0 && max_plan_length > 0) + { + robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose; + robot_geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose; + plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2)); + if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double egde_angle = atan2(p1.y - p2.y, p1.x - p2.x); + const double target_direction = cos(fabs(egde_angle - p1.theta)); + if (target_direction * x_direction_saved < 0.0) + { + plan_length = 0; + } + x_direction_saved = target_direction; + } + } + ++i; + } + } + catch (tf3::LookupException &ex) + { + robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what()); + return false; + } + catch (tf3::ConnectivityException &ex) + { + robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what()); + return false; + } + catch (tf3::ExtrapolationException &ex) + { + robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what()); + if (global_plan.poses.size() > 0) + robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); + + return false; + } + return true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( + const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result) +{ + const double dt = control_duration_; + + // --- X axis --- + double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); + double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); + + double vx = cmd_vel.x > 0.0 ? std::min(max_vel_x, std::max(min_vel_x, cmd_vel.x)) : std::max(-max_vel_x, std::min(-min_vel_x, cmd_vel.x)); + + double max_acc_vx = velocity.x + fabs(acc_lim_x_) * dt; + double min_acc_vx = velocity.x - fabs(decel_lim_x_) * dt; + vx = std::clamp(vx, min_acc_vx, max_acc_vx); + + // --- Y axis --- + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double vy = cmd_vel.y > 0.0 ? std::min(max_vel_y, std::max(min_vel_y, cmd_vel.y)) : std::max(-max_vel_y, std::min(-min_vel_y, cmd_vel.y)); + + double max_acc_vy = velocity.y + fabs(acc_lim_y_) * dt; + double min_acc_vy = velocity.y - fabs(decel_lim_y_) * dt; + vy = std::clamp(vy, min_acc_vy, max_acc_vy); + // --- Assign result --- + cmd_vel_result.x = vx; + cmd_vel_result.y = vy; + // cmd_vel_result.theta = vth; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) +{ + // slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible + // but we'll use a tenth of a second to be consistent with the implementation of the local planner. + const double dt = control_duration_; + double vx = sign(velocity.x) * std::max(0.0, (fabs(velocity.x) - fabs(decel_lim_x_) * dt)); + double vy = sign(velocity.y) * std::max(0.0, (fabs(velocity.y) - fabs(decel_lim_y_) * dt)); + + double vel_yaw = velocity.theta; + double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt)); + + // we do want to check whether or not the command is valid + cmd_vel.x = vx; + cmd_vel.y = vy; + cmd_vel.theta = vth; + return true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( + const double &dist_error, const double &lookahead_dist, + const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, + const double &pose_cost, double &linear_vel, double &sign_x) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + // std::stringstream ss; + // ss << linear_vel << " "; + // limit the linear velocity by curvature + if (use_regulated_linear_velocity_scaling_) + { + const double &min_rad = regulated_linear_scaling_min_radius_; + const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad; + if (radius < min_rad) + { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = curvature_vel; + this->moveWithAccLimits(velocity, cmd, result); + curvature_vel = result.x; + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + } + } + // ss << linear_vel << " "; + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(robot_costmap_2d::NO_INFORMATION) && + pose_cost != static_cast(robot_costmap_2d::FREE_SPACE)) + { + const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) + { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = cost_vel; + this->moveWithAccLimits(velocity, cmd, result); + cost_vel = result.x; + linear_vel = std::min(cost_vel, curvature_vel); + } + // ss << linear_vel << " "; + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop. + // This expression is eq. to + // (1) holding time to goal, t, constant using the theoretical + // lookahead distance and proposed velocity and + // (2) using t with the actual lookahead + // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). + + double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr + ? 2.0 * costmap_robot_->getCostmap()->getResolution() + : 0.1; + if (dist_error > dist_error_limit) + { + double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0; + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) + { + approach_vel = min_approach_linear_velocity_; + } + else + { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = approach_vel; + this->moveWithAccLimits(velocity, cmd, result); + approach_vel = result.x; + linear_vel = std::min(linear_vel, approach_vel); + } + + // ss << linear_vel << " "; + + // Limit linear velocities to be valid + double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); + double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y); + double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y); + double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel); + linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel); + linear_vel = sign_x * linear_vel; + // ss << linear_vel << " "; + // ROS_INFO_STREAM_THROTTLE(0.1, ss.str()); +} + +std::vector +mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution) +{ + + // Dịch sang tọa độ tuyệt đối + std::vector abs_footprint; + double cos_th = std::cos(pose.theta); + double sin_th = std::sin(pose.theta); + for (auto pt : footprint) + { + pt.x *= 1.2; + pt.y *= 1.2; + robot_geometry_msgs::Point abs_pt; + abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th; + abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th; + abs_footprint.push_back(abs_pt); + } + + std::vector points; + for (size_t i = 0; i < abs_footprint.size(); ++i) + { + const robot_geometry_msgs::Point &start = abs_footprint[i]; + const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; + + double dx = end.x - start.x; + double dy = end.y - start.y; + double distance = std::hypot(dx, dy); + int steps = std::max(1, static_cast(std::floor(distance / resolution))); + + for (int j = 0; j <= steps; ++j) + { + if (j == steps && i != abs_footprint.size() - 1) + continue; // tránh lặp + double t = static_cast(j) / steps; + robot_geometry_msgs::Point pt; + pt.x = start.x + t * dx; + pt.y = start.y + t * dy; + points.push_back(pt); + } + } + return points; +} + +double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y) +{ + unsigned int mx, my; + unsigned char cost; + if (costmap_robot_->getCostmap()->worldToMap(x, y, mx, my)) + { + cost = costmap_robot_->getCostmap()->getCost(mx, my); + } + return static_cast(cost); +} + +void mkt_algorithm::diff::PredictiveTrajectory::updateCostAtOffset( + TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose, + double x_offset, double y_offset, double &cost_left, double &cost_right) +{ + robot_nav_2d_msgs::Pose2DStamped stamped_pose, transformed_pose; + stamped_pose = base_pose; + stamped_pose.pose.x += x_offset; + stamped_pose.pose.y += y_offset; + + if (robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, transformed_pose)) + { + double cost = this->costAtPose(stamped_pose.pose.x, stamped_pose.pose.y); + if (transformed_pose.pose.y < 0) + cost_right = std::max(cost_right, cost); + else if (transformed_pose.pose.y > 0) + cost_left = std::max(cost_left, cost); + } +} + +double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + double journey_plan) +{ + double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + + carrot_pose.pose.y * carrot_pose.pose.y); + + // Avoid division by zero and handle backward motion + if (carrot_distance < 1e-3) + return journey_plan; + + // Project remaining path onto carrot direction + double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle) + return journey_plan * std::max(0.0, alignment); // Only positive projection +} + +bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::vector &angle_history, double current_angle, + double amplitude_threshold, size_t window_size) +{ + + angle_history.push_back(current_angle); + if ((unsigned int)angle_history.size() > window_size) + angle_history.erase(angle_history.begin()); + + if ((unsigned int)angle_history.size() < 2) + return false; + + double max_angle = *std::max_element(angle_history.begin(), angle_history.end()); + double min_angle = *std::min_element(angle_history.begin(), angle_history.end()); + + double amplitude = max_angle - min_angle; + // if(fabs(amplitude) > amplitude_threshold) + // ROS_INFO_THROTTLE(0.1,"%f %f %f %d %d", amplitude, max_angle , min_angle, (unsigned int)angle_history.size(), (unsigned int)window_size); + return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; +} + +void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose) +{ + const auto &plan_back_pose = compute_plan_.poses.back(); + // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0; + // const double offset_min = this->min_path_distance_; + + auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points_rb) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - pose.pose.x; + double dy = point.y - pose.pose.y; + double cos_yaw = cos(-pose.pose.theta); + double sin_yaw = sin(-pose.pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_side_ = std::max(cost_left_side_, cost_goal); + } + else if (y_rel < -epsilon) + { + cost_right_side_ = std::max(cost_right_side_, cost_goal); + } + } + + unsigned int step_footprint = 10; + if ((unsigned int)(compute_plan_.poses.size() - 1) < 10) + { + auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - plan_back_pose.pose.x; + double dy = point.y - plan_back_pose.pose.y; + double cos_yaw = cos(-plan_back_pose.pose.theta); + double sin_yaw = sin(-plan_back_pose.pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_goal_ = std::max(cost_left_goal_, cost_goal); + } + else if (y_rel < -epsilon) + { + cost_right_goal_ = std::max(cost_right_goal_, cost_goal); + } + else + center_cost_ = std::max(center_cost_, cost_goal); + } + } + else + { + for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint) + { + auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - compute_plan_.poses[i].pose.x; + double dy = point.y - compute_plan_.poses[i].pose.y; + double cos_yaw = cos(-compute_plan_.poses[i].pose.theta); + double sin_yaw = sin(-compute_plan_.poses[i].pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_goal_ = std::max(cost_left_goal_, cost_goal); + } + else if (y_rel < -epsilon) + { + cost_right_goal_ = std::max(cost_right_goal_, cost_goal); + } + else + center_cost_ = std::max(center_cost_, cost_goal); + } + double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x; + double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y; + if (hypot(dx, dy) > 1.0) + break; + } + } +} + +score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() +{ + return std::make_shared(); +} + +// Register this planner as a GlobalPlanner plugin +BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp new file mode 100644 index 0000000..bf87617 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp @@ -0,0 +1,1320 @@ +#include +#include +#include + +#define LIMIT_VEL_THETA 0.33 + +void mkt_algorithm::diff::PredictiveTrajectory::initialize( + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) +{ + if (!initialized_) + { + nh_ = robot::NodeHandle("~"); + nh_priv_ = robot::NodeHandle(nh, name); + name_ = name; + tf_ = tf; + traj_ = traj; + costmap_robot_ = costmap_robot; + this->getParams(); + nh_.param("publish_topic", enable_publish_, false); + nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); + + footprint_spec_ = costmap_robot_->getRobotFootprint(); + std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const robot_geometry_msgs::Point &p1 = footprint[i]; + const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + this->initKalmanFilter(); + x_direction_ = y_direction_ = theta_direction_ = 0; + this->initialized_ = true; + robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); + } +} + +mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() +{ + if (kf_) + { + kf_.reset(); + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter() +{ + // kalman + last_actuator_update_ = robot::Time::now(); + n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] + m_ = 2; // measurements: x, y, theta + 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_); + + 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; + } + + C(0, 0) = 1; + C(1, 3) = 1; + Q(2, 2) = 0.1; + Q(5, 5) = 0.6; + + 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; + + kf_ = boost::make_shared(dt, A, C, Q, R, P); + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); +} + +void mkt_algorithm::diff::PredictiveTrajectory::getParams() +{ + robot_base_frame_ = nh_priv_.param("robot_base_frame", std::string("base_link")); + nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); + nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); + nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); + nh_priv_.param("index_samples", index_samples_, 0); + nh_priv_.param("follow_step_path", follow_step_path_, false); + + nh_priv_.param("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false); + nh_priv_.param("lookahead_dist", lookahead_dist_, 0.25); + nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.3); + nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 0.9); + nh_priv_.param("lookahead_time", lookahead_time_, 1.5); + nh_priv_.param("min_journey_squared", min_journey_squared_, std::numeric_limits::infinity()); + nh_priv_.param("max_journey_squared", max_journey_squared_, std::numeric_limits::infinity()); + + nh_priv_.param("use_rotate_to_heading", use_rotate_to_heading_, false); + nh_priv_.param("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785); + + nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); + nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); + if (trans_stopped_velocity_ <= min_approach_linear_velocity_ || trans_stopped_velocity_ - min_approach_linear_velocity_ < 0.02) + trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.05; + + // Regulated linear velocity scaling + nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); + nh_priv_.param("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9); + nh_priv_.param("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25); + + // Inflation cost scaling (Limit velocity by proximity to obstacles) + nh_priv_.param("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true); + nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); + nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); + nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); + if (inflation_cost_scaling_factor_ <= 0.0) + { + 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."); + use_cost_regulated_linear_velocity_scaling_ = false; + } + double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + control_duration_ = 1.0 / control_frequency; + window_size_ = (size_t)(control_frequency * 3.0); + + if (traj_) + { + traj_.get()->getNodeHandle().param("max_vel_x", max_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_x", min_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_x", acc_lim_x_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_x", decel_lim_x_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_y", max_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_y", min_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_y", acc_lim_y_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_y", decel_lim_y_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_theta", max_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::reset() +{ + if (this->initialized_) + { + robot::log_info("[%s:%d]\n PredictiveTrajectory is reset", __FILE__, __LINE__); + reached_intermediate_goals_.clear(); + start_index_saved_vt_.clear(); + this->clear(); + x_direction_ = y_direction_ = theta_direction_ = 0; + this->follow_step_path_ = false; + this->nav_stop_ = false; + last_actuator_update_ = robot::Time::now(); + prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); + + if (kf_) + { + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); + } + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::stop() +{ + this->nav_stop_ = true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::resume() +{ + if (this->initialized_) + { + if(!this->nav_stop_) + return; + prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); + last_actuator_update_ = robot::Time::now(); + + if (kf_) + { + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); + } + this->nav_stop_ = false; + } +} + +bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) +{ + if (!initialized_) + { + robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); + return false; + } + + std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const robot_geometry_msgs::Point &p1 = footprint[i]; + const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + + if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) + { + robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); + return false; + } + this->getParams(); + + frame_id_path_ = global_plan.header.frame_id; + goal_ = goal; + global_plan_ = global_plan; + + // prune global plan to cut off parts of the past (spatially before the robot) + if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0)) + { + robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__); + return false; + } + + double S = std::numeric_limits::infinity(); + S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0, + costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0); + const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_; + S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S); + compute_plan_.poses.clear(); + + if ((unsigned int)global_plan_.poses.size() == 2) + { + double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x; + double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y; + if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution()) + { + compute_plan_ = global_plan_; + } + else + { + robot::log_error("[%s:%d]\n The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution()); + return false; + } + } + else + { + unsigned int start_index, goal_index; + if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_)) + { + robot::log_error("[%s:%d]\n computePlanCommand is Failed", __FILE__, __LINE__); + return false; + } + } + + double lookahead_dist = this->getLookAheadDistance(velocity); + transform_plan_.poses.clear(); + if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_)) + { + robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); + return false; + } + // else + // { + // robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); + // } + + x_direction = x_direction_; + y_direction = y_direction_ = 0; + theta_direction = theta_direction_; + + if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq) + { + const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose; + int index; + for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--) + { + robot_geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose; + if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_) + break; + } + const robot_geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose; + if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x); + const double dir_path = cos(fabs(path_angle - p2.theta)); + if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) + x_direction = dir_path > 0 ? FORWARD : BACKWARD; + } + } + else + { + try + { + auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_); + auto prev_carrot_pose_it = transform_plan_.poses.begin(); + double distance_it = 0; + for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) + { + double dx = it->pose.x - carrot_pose_it->pose.x; + double dy = it->pose.x - carrot_pose_it->pose.y; + distance_it += std::hypot(dx, dy); + if (distance_it > costmap_robot_->getCostmap()->getResolution()) + { + prev_carrot_pose_it = it; + break; + } + } + + robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution() + ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) + : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D()); + + robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); + + // teb_local_planner::PoseSE2 start_pose(front); + // teb_local_planner::PoseSE2 goal_pose(back); + // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec()); + const double dir_path = 0.0; + + if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) + x_direction = dir_path > 0 ? FORWARD : BACKWARD; + } + catch (std::exception &e) + { + robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what()); + return false; + } + } + + x_direction_ = x_direction; + y_direction_ = y_direction; + theta_direction_ = theta_direction; + return true; +} + +mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) +{ + mkt_msgs::Trajectory2D result; + result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; + if (!traj_) + return result; + if (compute_plan_.poses.size() < 2) + { + robot::log_warning("[%s:%d]\n Local compute plan is available", __FILE__, __LINE__); + return result; + } + + robot::Time current_time = robot::Time::now(); + double dt = (current_time - last_actuator_update_).toSec(); + last_actuator_update_ = current_time; + + robot_nav_2d_msgs::Twist2D drive_cmd; + double sign_x = sign(x_direction_); + robot_nav_2d_msgs::Twist2D twist; + robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta); + traj_->startNewIteration(velocity); + while (robot::ok() && traj_->hasMoreTwists()) + { + twist = traj_->nextTwist(); + } + drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); + robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta); + robot::log_info("--------------------------------"); + double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + + robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; + result.poses.clear(); + result.poses.reserve(transformed_plan.poses.size()); + for (const auto &pose_stamped : transformed_plan.poses) + { + result.poses.push_back(pose_stamped.pose); + } + + if (transformed_plan.poses.empty()) + { + robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); + return result; + } + + double lookahead_dist = getLookAheadDistance(velocity); + double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y); + + if (transformed_plan.poses.empty()) + { + robot::log_warning("[%s:%d]\n Transformed plan is empty after compute lookahead point", __FILE__, __LINE__); + return result; + } + auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); + bool allow_rotate = false; + nh_priv_.param("allow_rotate", allow_rotate, false); + + // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y); + robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose; + const double distance_allow_rotate = min_journey_squared_; + const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y); + const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1); + if (avoid_obstacles_) + allow_rotate = journey_plan >= distance_allow_rotate && + fabs(front.y) <= 0.2 && + (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution()); + else + allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; + + double angle_to_heading; + if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) + { + if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + { + rotateToHeading(angle_to_heading, velocity, drive_cmd); + } + } + else + { + const double vel_x_reduce = std::min(fabs(v_max), 0.3); + double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; + carrot_dist2 = std::max(carrot_dist2, 0.05); + double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + + const auto &plan_back_pose = compute_plan_.poses.back(); + double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y); + post_cost = std::max(post_cost, center_cost_); + this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); + + const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; + const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; + double d_reduce = std::clamp(journey_plan, min_S, max_S); + double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); + double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); + double v_min = + journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; + v_min *= sign_x; + + double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); + double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); + double vel_reduce = sign_x > 0 + ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) + : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); + drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; + + double v_theta = drive_cmd.x * curvature; + double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_)) + { + carrot_dist2 *= 0.6; + curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + v_theta = drive_cmd.x * curvature; + } + if (fabs(v_theta) > LIMIT_VEL_THETA) + { + robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result; + cmd_vel.x = sign_x > 0 + ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1)) + : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1)); + cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5); + this->moveWithAccLimits(velocity, cmd_vel, cmd_result); + drive_cmd.x = std::copysign(cmd_result.x, sign_x); + v_theta = drive_cmd.x * curvature; + } + + if (journey_plan < min_journey_squared_) + { + if (transform_plan_.poses.size() > 2) + { + robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); + robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; + double dx = end.pose.x - start.pose.x; + double dy = end.pose.y - start.pose.y; + v_theta = atan2(dy, dx); + if (v_theta > M_PI_2) + v_theta -= M_PI; + else if (v_theta < -M_PI_2) + v_theta += M_PI; + // v_theta *= 0.5; + v_theta = std::clamp(v_theta, -0.02, 0.02); + } + else + v_theta = 0.0; + } + double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8; + double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; + double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; + drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); + + if (this->nav_stop_) + { + if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + drive_cmd = {}; + 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); + + // Check if Kalman filter's estimated velocity exceeds v_max + if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || + cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || + cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE || + cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE)) + { + drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0]; + } + else + { + drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); + } + + drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x); + drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA); + } + + result.velocity = drive_cmd; + prevous_drive_cmd_ = drive_cmd; + return result; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( + const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x) +{ + bool curvature = false; + double path_angle = 0; + if (!global_plan.poses.empty()) + { + if ((unsigned)global_plan.poses.size() > 2) + { + const double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + const double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + { + double start_angle = atan2(start_direction_y, start_direction_x); + for (unsigned int i = 1; i < (unsigned)global_plan.poses.size(); i++) + { + const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; + const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; + if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + { + double current_angle = atan2(current_direction_y, current_direction_x); + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > 1e-9) + { + curvature = true; + break; + } + path_angle = current_angle; + } + } + } + } + } + // ROS_INFO_THROTTLE(0.1,"path_angle %.3f %.3f", path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x)); + // Whether we should rotate robot to rough path heading + angle_to_path = curvature ? path_angle : std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + angle_to_path = sign_x < 0 + ? angles::normalize_angle(M_PI + angle_to_path) + : angles::normalize_angle(angle_to_path); + + double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y); + // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + double heading_rotate = rotate_to_heading_min_angle_; + bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_); + heading_rotate = is_stopped + ? rotate_to_heading_min_angle_ + heading_linear + : std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2); + + bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate; + + // if (result) + // { + // ROS_WARN_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f", + // velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_); + // ROS_WARN_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n", + // is_stopped, path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x), angle_to_path, heading_rotate, sign_x); + // } + // else + // { + // ROS_INFO_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f", + // velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_); + // ROS_INFO_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n", + // is_stopped, path_angle,std::atan2(carrot_pose.pose.y, carrot_pose.pose.x) ,angle_to_path, heading_rotate, sign_x); + // } + + return result; +} + +void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) +{ + const double dt = control_duration_; + const double theta_direction = angle_to_path > 0.0 ? 1.0 : -1.0; + double vel_yaw = theta_direction * fabs(velocity.theta); + double ang_diff = angle_to_path; + double max_vel_theta = traj_ ? fabs(traj_->getTwistAngular(true).z) : 0.3; + + double angular_decel_zone = 0.3; // rad - khoảng cách bắt đầu giảm tốc + nh_priv_.param("angular_decel_zone", angular_decel_zone, 0.5); + angular_decel_zone += std::clamp( + fabs(max_vel_theta) + (M_PI_2 - fabs(atan2(min_path_distance_, max_path_distance_))), 0.4, M_PI_2); + double zone_begin = std::max(angular_decel_zone * 0.2, 0.2); + double cosine_begin_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / zone_begin))); + + double reduce_vel_theta = std::min(max_vel_theta, 0.25 + min_vel_theta_); + reduce_vel_theta = fabs(ang_diff) > zone_begin ? reduce_vel_theta : (reduce_vel_theta - min_vel_theta_) * cosine_begin_factor + min_vel_theta_; + + // === ANGULAR DECELERATION ZONE === + double target_angular_speed = max_vel_theta; + if (fabs(ang_diff) < angular_decel_zone) + { + // ROS_WARN_THROTTLE(0.2,"%f %f %f %f", ang_diff, angular_decel_zone, zone_begin, max_vel_theta); + double cosine_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / angular_decel_zone))); + double cosine_speed = max_vel_theta * cosine_factor; + + // Choose deceleration profile + target_angular_speed = cosine_speed; // Smoothest option + + // Ensure minimum speed to avoid stalling + target_angular_speed = std::max(target_angular_speed, reduce_vel_theta); + } + // else + // ROS_INFO_THROTTLE(1,"%f %f %f %f", ang_diff, angular_decel_zone, max_vel_theta, reduce_vel_theta); + + // Apply direction + double v_theta_desired = std::copysign(target_angular_speed, ang_diff); + + // === ACCELERATION LIMITS === + double max_acc_vel = vel_yaw + fabs(acc_lim_theta_) * dt; + double min_acc_vel = vel_yaw - fabs(acc_lim_theta_) * dt; + double v_theta_samp = std::clamp(v_theta_desired, min_acc_vel, max_acc_vel); + + // === STOPPING CONSTRAINT === + double max_speed_to_stop = sqrt(2.0 * fabs(decel_lim_theta_) * fabs(ang_diff)); + v_theta_samp = std::copysign( + std::min(max_speed_to_stop, fabs(v_theta_samp)), ang_diff); + + // === FINAL LIMITS === + v_theta_samp = theta_direction > 0 + ? std::clamp(v_theta_samp, min_vel_theta_, max_vel_theta) + : std::clamp(v_theta_samp, -max_vel_theta, -min_vel_theta_); + + cmd_vel.x = 0.0; + cmd_vel.y = 0.0; + cmd_vel.theta = v_theta_samp; +} + +double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) + { + lookahead_dist = fabs(velocity.x) * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +std::vector::iterator +mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan) +{ + if (global_plan.poses.empty()) + { + throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty."); + } + + if ((unsigned)global_plan.poses.size() < 2) + { + auto goal_pose_it = std::prev(global_plan.poses.end()); + return goal_pose_it; + } + unsigned int goal_index = (unsigned)global_plan.poses.size() - 1; + + double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; + double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; + + // make sure that atan2 is defined + double start_angle = atan2(start_direction_y, start_direction_x); + double end_angle = atan2(end_direction_y, end_direction_x); + double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); + + for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) + { + if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + { + const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; + const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; + + if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + { + double current_angle = atan2(current_direction_y, current_direction_x); + goal_index = i; + if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) + continue; + + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) + { + goal_index = i; + break; + } + } + } + } + + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + global_plan.poses.begin(), global_plan.poses.begin() + goal_index, + [&](const auto &ps) + { + return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; + }); + + // If the number of poses is not far enough, take the last pose + if (goal_pose_it == global_plan.poses.end()) + { + goal_pose_it = std::prev(global_plan.poses.end()); + } + return goal_pose_it; +} + +robot_geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose) +{ + robot_geometry_msgs::PointStamped carrot_msg; + carrot_msg.header = carrot_pose.header; + carrot_msg.point.x = carrot_pose.pose.x; + carrot_msg.point.y = carrot_pose.pose.y; + carrot_msg.point.z = 0.5; // publish right over map to stand out + return carrot_msg; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot) +{ + if (global_plan.poses.empty()) + return false; + if (tf == nullptr) + return false; + try + { + // let's get the pose of the robot in the frame of the plan + robot_nav_2d_msgs::Pose2DStamped robot; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) + { + throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame"); + } + + double dist_thresh_sq = dist_behind_robot * dist_behind_robot; + + // iterate plan until a pose close the robot is found + std::vector::iterator it = global_plan.poses.begin(); + std::vector::iterator erase_end = it; + while (it != global_plan.poses.end()) + { + double dx = robot.pose.x - it->pose.x; + double dy = robot.pose.y - it->pose.y; + double dist_sq = dx * dx + dy * dy; + if (dist_sq < dist_thresh_sq) + { + erase_end = it; + break; + } + ++it; + } + if (erase_end == global_plan.poses.end()) + return false; + + if (erase_end != global_plan.poses.begin()) + global_plan.poses.erase(global_plan.poses.begin(), erase_end); + } + catch (const tf3::TransformException &ex) + { + robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what()); + return false; + } + return true; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( + TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, + robot_nav_2d_msgs::Path2D &transformed_plan) +{ + // this method is a slightly modified version of base_local_planner/goal_functions.h + transformed_plan.poses.clear(); + transformed_plan.header.stamp = pose.header.stamp; + transformed_plan.header.frame_id = robot_base_frame; + + if (global_plan.poses.empty()) + return false; + + if (tf == nullptr) + return false; + + if (costmap == nullptr || costmap->getCostmap() == nullptr) + return false; + + try + { + if (global_plan.poses.empty()) + { + robot::log_error("[%s:%d]\n Received plan with zero length", __FILE__, __LINE__); + return false; + } + + // let's get the pose of the robot in the frame of the plan + robot_nav_2d_msgs::Pose2DStamped robot_pose; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) + { + throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame"); + } + + // we'll discard points on the plan that are outside the local costmap + double dist_threshold = std::max(costmap->getCostmap()->getSizeInCellsX() * costmap->getCostmap()->getResolution() / 2.0, + costmap->getCostmap()->getSizeInCellsY() * costmap->getCostmap()->getResolution() / 2.0); + + dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are + double sq_dist_threshold = dist_threshold * dist_threshold; + // located on the border of the local costmap + + int i = 0; + double sq_dist = std::numeric_limits::infinity(); + + // we need to loop to a point on the plan that is within a certain distance of the robot + bool robot_reached = false; + for (int j = 0; j < (int)global_plan.poses.size(); ++j) + { + double x_diff = robot_pose.pose.x - global_plan.poses[j].pose.x; + double y_diff = robot_pose.pose.y - global_plan.poses[j].pose.y; + double new_sq_dist = x_diff * x_diff + y_diff * y_diff; + if (robot_reached && new_sq_dist > sq_dist_threshold) + { + break; + } // force stop if we have reached the costmap border + + if (new_sq_dist < sq_dist) // find closest distance + { + sq_dist = new_sq_dist; + i = j; + if (sq_dist < costmap->getCostmap()->getResolution()) // 5 cm to the robot; take the immediate local minima; if it's not the global + robot_reached = true; // minima, probably means that there's a loop in the path, and so we prefer this + } + } + + double plan_length = 0; // check cumulative Euclidean distance along the plan + double x_direction_saved = 0.0; + // now we'll transform until points are outside of our distance threshold + while (i < (int)global_plan.poses.size() && sq_dist <= sq_dist_threshold && + (max_plan_length <= 0 || plan_length <= max_plan_length)) + { + robot_nav_2d_msgs::Pose2DStamped stamped_pose; + stamped_pose.pose = global_plan.poses[i].pose; + stamped_pose.header.frame_id = global_plan.header.frame_id; + + robot_nav_2d_msgs::Pose2DStamped newer_pose; + if (!robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, newer_pose)) + { + ++i; + continue; + } + transformed_plan.poses.push_back(newer_pose); + + double x_diff = robot_pose.pose.x - global_plan.poses[i].pose.x; + double y_diff = robot_pose.pose.y - global_plan.poses[i].pose.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + // Calculate distance to previous pose + if (i > 0 && max_plan_length > 0) + { + robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose; + robot_geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose; + plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2)); + if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double egde_angle = atan2(p1.y - p2.y, p1.x - p2.x); + const double target_direction = cos(fabs(egde_angle - p1.theta)); + if (target_direction * x_direction_saved < 0.0) + { + plan_length = 0; + } + x_direction_saved = target_direction; + } + } + ++i; + } + } + catch (tf3::LookupException &ex) + { + robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what()); + return false; + } + catch (tf3::ConnectivityException &ex) + { + robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what()); + return false; + } + catch (tf3::ExtrapolationException &ex) + { + robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what()); + if (global_plan.poses.size() > 0) + robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); + + return false; + } + return true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( + const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result) +{ + const double dt = control_duration_; + + // --- X axis --- + double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); + double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); + + double vx = cmd_vel.x > 0.0 ? std::min(max_vel_x, std::max(min_vel_x, cmd_vel.x)) : std::max(-max_vel_x, std::min(-min_vel_x, cmd_vel.x)); + + double max_acc_vx = velocity.x + fabs(acc_lim_x_) * dt; + double min_acc_vx = velocity.x - fabs(decel_lim_x_) * dt; + vx = std::clamp(vx, min_acc_vx, max_acc_vx); + + // --- Y axis --- + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double vy = cmd_vel.y > 0.0 ? std::min(max_vel_y, std::max(min_vel_y, cmd_vel.y)) : std::max(-max_vel_y, std::min(-min_vel_y, cmd_vel.y)); + + double max_acc_vy = velocity.y + fabs(acc_lim_y_) * dt; + double min_acc_vy = velocity.y - fabs(decel_lim_y_) * dt; + vy = std::clamp(vy, min_acc_vy, max_acc_vy); + // --- Assign result --- + cmd_vel_result.x = vx; + cmd_vel_result.y = vy; + // cmd_vel_result.theta = vth; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) +{ + // slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible + // but we'll use a tenth of a second to be consistent with the implementation of the local planner. + const double dt = control_duration_; + double vx = sign(velocity.x) * std::max(0.0, (fabs(velocity.x) - fabs(decel_lim_x_) * dt)); + double vy = sign(velocity.y) * std::max(0.0, (fabs(velocity.y) - fabs(decel_lim_y_) * dt)); + + double vel_yaw = velocity.theta; + double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt)); + + // we do want to check whether or not the command is valid + cmd_vel.x = vx; + cmd_vel.y = vy; + cmd_vel.theta = vth; + return true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( + const double &dist_error, const double &lookahead_dist, + const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, + const double &pose_cost, double &linear_vel, double &sign_x) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + // std::stringstream ss; + // ss << linear_vel << " "; + // limit the linear velocity by curvature + if (use_regulated_linear_velocity_scaling_) + { + const double &min_rad = regulated_linear_scaling_min_radius_; + const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad; + if (radius < min_rad) + { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = curvature_vel; + this->moveWithAccLimits(velocity, cmd, result); + curvature_vel = result.x; + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + } + } + // ss << linear_vel << " "; + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(robot_costmap_2d::NO_INFORMATION) && + pose_cost != static_cast(robot_costmap_2d::FREE_SPACE)) + { + const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) + { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = cost_vel; + this->moveWithAccLimits(velocity, cmd, result); + cost_vel = result.x; + linear_vel = std::min(cost_vel, curvature_vel); + } + // ss << linear_vel << " "; + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop. + // This expression is eq. to + // (1) holding time to goal, t, constant using the theoretical + // lookahead distance and proposed velocity and + // (2) using t with the actual lookahead + // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). + + double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr + ? 2.0 * costmap_robot_->getCostmap()->getResolution() + : 0.1; + if (dist_error > dist_error_limit) + { + double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0; + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) + { + approach_vel = min_approach_linear_velocity_; + } + else + { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = approach_vel; + this->moveWithAccLimits(velocity, cmd, result); + approach_vel = result.x; + linear_vel = std::min(linear_vel, approach_vel); + } + + // ss << linear_vel << " "; + + // Limit linear velocities to be valid + double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); + double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y); + double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y); + double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel); + linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel); + linear_vel = sign_x * linear_vel; + // ss << linear_vel << " "; + // ROS_INFO_STREAM_THROTTLE(0.1, ss.str()); +} + +std::vector +mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution) +{ + + // Dịch sang tọa độ tuyệt đối + std::vector abs_footprint; + double cos_th = std::cos(pose.theta); + double sin_th = std::sin(pose.theta); + for (auto pt : footprint) + { + pt.x *= 1.2; + pt.y *= 1.2; + robot_geometry_msgs::Point abs_pt; + abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th; + abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th; + abs_footprint.push_back(abs_pt); + } + + std::vector points; + for (size_t i = 0; i < abs_footprint.size(); ++i) + { + const robot_geometry_msgs::Point &start = abs_footprint[i]; + const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; + + double dx = end.x - start.x; + double dy = end.y - start.y; + double distance = std::hypot(dx, dy); + int steps = std::max(1, static_cast(std::floor(distance / resolution))); + + for (int j = 0; j <= steps; ++j) + { + if (j == steps && i != abs_footprint.size() - 1) + continue; // tránh lặp + double t = static_cast(j) / steps; + robot_geometry_msgs::Point pt; + pt.x = start.x + t * dx; + pt.y = start.y + t * dy; + points.push_back(pt); + } + } + return points; +} + +double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y) +{ + unsigned int mx, my; + unsigned char cost; + if (costmap_robot_->getCostmap()->worldToMap(x, y, mx, my)) + { + cost = costmap_robot_->getCostmap()->getCost(mx, my); + } + return static_cast(cost); +} + +void mkt_algorithm::diff::PredictiveTrajectory::updateCostAtOffset( + TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose, + double x_offset, double y_offset, double &cost_left, double &cost_right) +{ + robot_nav_2d_msgs::Pose2DStamped stamped_pose, transformed_pose; + stamped_pose = base_pose; + stamped_pose.pose.x += x_offset; + stamped_pose.pose.y += y_offset; + + if (robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, transformed_pose)) + { + double cost = this->costAtPose(stamped_pose.pose.x, stamped_pose.pose.y); + if (transformed_pose.pose.y < 0) + cost_right = std::max(cost_right, cost); + else if (transformed_pose.pose.y > 0) + cost_left = std::max(cost_left, cost); + } +} + +double mkt_algorithm::diff::PredictiveTrajectory::computeDecelerationFactor(double remaining_distance, double decel_distance) +{ + if (remaining_distance >= decel_distance) + { + return 1.0; // Full speed + } + + // Smooth transition using cosine function + double ratio = fabs(remaining_distance / decel_distance); + return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0 +} + +double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + double journey_plan) +{ + double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + + carrot_pose.pose.y * carrot_pose.pose.y); + + // Avoid division by zero and handle backward motion + if (carrot_distance < 1e-3) + return journey_plan; + + // Project remaining path onto carrot direction + double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle) + return journey_plan * std::max(0.0, alignment); // Only positive projection +} + +bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::vector &angle_history, double current_angle, + double amplitude_threshold, size_t window_size) +{ + + angle_history.push_back(current_angle); + if ((unsigned int)angle_history.size() > window_size) + angle_history.erase(angle_history.begin()); + + if ((unsigned int)angle_history.size() < 2) + return false; + + double max_angle = *std::max_element(angle_history.begin(), angle_history.end()); + double min_angle = *std::min_element(angle_history.begin(), angle_history.end()); + + double amplitude = max_angle - min_angle; + // if(fabs(amplitude) > amplitude_threshold) + // ROS_INFO_THROTTLE(0.1,"%f %f %f %d %d", amplitude, max_angle , min_angle, (unsigned int)angle_history.size(), (unsigned int)window_size); + return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; +} + +void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose) +{ + const auto &plan_back_pose = compute_plan_.poses.back(); + // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0; + // const double offset_min = this->min_path_distance_; + + auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points_rb) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - pose.pose.x; + double dy = point.y - pose.pose.y; + double cos_yaw = cos(-pose.pose.theta); + double sin_yaw = sin(-pose.pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_side_ = std::max(cost_left_side_, cost_goal); + } + else if (y_rel < -epsilon) + { + cost_right_side_ = std::max(cost_right_side_, cost_goal); + } + } + + unsigned int step_footprint = 10; + if ((unsigned int)(compute_plan_.poses.size() - 1) < 10) + { + auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - plan_back_pose.pose.x; + double dy = point.y - plan_back_pose.pose.y; + double cos_yaw = cos(-plan_back_pose.pose.theta); + double sin_yaw = sin(-plan_back_pose.pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_goal_ = std::max(cost_left_goal_, cost_goal); + } + else if (y_rel < -epsilon) + { + cost_right_goal_ = std::max(cost_right_goal_, cost_goal); + } + else + center_cost_ = std::max(center_cost_, cost_goal); + } + } + else + { + for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint) + { + auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - compute_plan_.poses[i].pose.x; + double dy = point.y - compute_plan_.poses[i].pose.y; + double cos_yaw = cos(-compute_plan_.poses[i].pose.theta); + double sin_yaw = sin(-compute_plan_.poses[i].pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_goal_ = std::max(cost_left_goal_, cost_goal); + } + else if (y_rel < -epsilon) + { + cost_right_goal_ = std::max(cost_right_goal_, cost_goal); + } + else + center_cost_ = std::max(center_cost_, cost_goal); + } + double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x; + double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y; + if (hypot(dx, dy) > 1.0) + break; + } + } +} + +score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() +{ + return std::make_shared(); +} + +// Register this planner as a GlobalPlanner plugin +BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory) \ No newline at end of file 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 70a6a79..bf87617 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 @@ -383,7 +383,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( robot::log_warning("[%s:%d]\n Local compute plan is available", __FILE__, __LINE__); return result; } - + robot::Time current_time = robot::Time::now(); double dt = (current_time - last_actuator_update_).toSec(); last_actuator_update_ = current_time; @@ -391,15 +391,25 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( robot_nav_2d_msgs::Twist2D drive_cmd; double sign_x = sign(x_direction_); robot_nav_2d_msgs::Twist2D twist; + robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta); traj_->startNewIteration(velocity); - while (traj_->hasMoreTwists()) + while (robot::ok() && traj_->hasMoreTwists()) { twist = traj_->nextTwist(); } drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); + robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta); + robot::log_info("--------------------------------"); double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; + result.poses.clear(); + result.poses.reserve(transformed_plan.poses.size()); + for (const auto &pose_stamped : transformed_plan.poses) + { + result.poses.push_back(pose_stamped.pose); + } + if (transformed_plan.poses.empty()) { robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp new file mode 100644 index 0000000..8201214 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp @@ -0,0 +1,52 @@ +#include + +void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity_min, + const double &velocity_max, + const double &linear_vel, + const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + const double &min_dist2, + const double &curvature_dist2_floor, + const double &dist2_override) +{ + const double vel_x_reduce = std::min(fabs(velocity_max), 0.3); + double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; + carrot_dist2 = std::max(carrot_dist2, 0.05); + double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + + if (max_lateral_accel_ > 1e-6) + { + const double curvature_abs = std::max(fabs(curvature), 1e-6); + const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs); + drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit); + } + + const auto &plan_back_pose = compute_plan_.poses.back(); + double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y); + post_cost = std::max(post_cost, center_cost_); + this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); + + const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; + const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; + double d_reduce = std::clamp(journey_plan, min_S, max_S); + double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); + double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); + double v_min = + journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; + v_min *= sign_x; + + double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); + double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); + double vel_reduce = sign_x > 0 + ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) + : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); + drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; + + double v_theta = drive_cmd.x * curvature; + double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_)) + { + carrot_dist2 *= 0.6; + curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + v_theta = drive_cmd.x * curvature; + } +} diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h index 319792a..253f43c 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h @@ -270,6 +270,23 @@ namespace mkt_plugins using Ptr = std::shared_ptr; protected: + + /** + * @brief Configure the kinematic parameters + * @param nh NodeHandle + * + * Configures the kinematic parameters from the given NodeHandle. + */ + void configure(const robot::NodeHandle &nh); + + /** + * @brief Reconfigure the kinematic parameters + * @param nh NodeHandle + * + * Reconfigures the kinematic parameters from the given NodeHandle. + */ + void reconfigure(const robot::NodeHandle &nh); + // For parameter descriptions, see cfg/KinematicParams.cfg int xytheta_direct_[3]; double min_vel_x_, min_vel_y_; diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h index eb6b396..b49cd6b 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h @@ -116,9 +116,13 @@ namespace mkt_plugins * current velocity within acc_time seconds, respecting acceleration/deceleration limits. * The velocities range from min_vel_ to max_vel_ (projected from current velocity). */ - OneDVelocityIterator(double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, - int num_samples) + OneDVelocityIterator( + std::string name, double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, int num_samples) + : name_(name) { + if(name_ == "x_it_") + robot::log_warning("[%s:%d] one_d_velocity_iterator: [%s] %f %d %f %f %f %f %f %d", + __FILE__, __LINE__, name_.c_str(), current, direct, min, max, acc_limit, decel_limit, acc_time, num_samples); // if (current < min) // { // current = min; @@ -203,8 +207,6 @@ namespace mkt_plugins */ bool isFinished() const { - // if(name_ == std::string("th_it_")) - // ROS_INFO("%s %f %f", name_.c_str(), current_, max_vel_ + EPSILON); double limit_vel = direct_ > 0? max_vel_ : min_vel_; return fabs(current_) > fabs(limit_vel) + EPSILON; } diff --git a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp index 84ba7ac..5af06df 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp @@ -54,10 +54,48 @@ namespace mkt_plugins setDecelerationAsNeeded(nh, "y"); setDecelerationAsNeeded(nh, "theta"); - + configure(nh); xytheta_direct_[0] = xytheta_direct_[1] = xytheta_direct_[2] = 1; } + + void KinematicParameters::configure(const robot::NodeHandle &nh) + { + nh.param("max_vel_x", original_max_vel_x_, 0.0); + nh.param("max_vel_y", original_max_vel_y_, 0.0); + nh.param("max_vel_theta", original_max_vel_theta_, 0.0); + nh.param("min_vel_x", original_min_vel_x_, 0.0); + nh.param("min_vel_y", original_min_vel_y_, 0.0); + nh.param("min_speed_xy", original_min_speed_xy_, 0.0); + nh.param("max_speed_xy", original_max_speed_xy_, 0.0); + nh.param("min_speed_theta", original_min_speed_theta_, 0.0); + nh.param("acc_lim_x", original_acc_lim_x_, 0.0); + nh.param("acc_lim_y", original_acc_lim_y_, 0.0); + nh.param("acc_lim_theta", original_acc_lim_theta_, 0.0); + nh.param("decel_lim_x", original_decel_lim_x_, 0.0); + nh.param("decel_lim_y", original_decel_lim_y_, 0.0); + nh.param("decel_lim_theta", original_decel_lim_theta_, 0.0); + reconfigure(nh); + } + + void KinematicParameters::reconfigure(const robot::NodeHandle &nh) + { + nh.param("max_vel_x", max_vel_x_, 0.0); + nh.param("max_vel_y", max_vel_y_, 0.0); + nh.param("max_vel_theta", max_vel_theta_, 0.0); + nh.param("min_vel_x", min_vel_x_, 0.0); + nh.param("min_vel_y", min_vel_y_, 0.0); + nh.param("min_speed_xy", min_speed_xy_, 0.0); + nh.param("max_speed_xy", max_speed_xy_, 0.0); + nh.param("min_speed_theta", min_speed_theta_, 0.0); + nh.param("acc_lim_x", acc_lim_x_, 0.0); + nh.param("acc_lim_y", acc_lim_y_, 0.0); + nh.param("acc_lim_theta", acc_lim_theta_, 0.0); + nh.param("decel_lim_x", decel_lim_x_, 0.0); + nh.param("decel_lim_y", decel_lim_y_, 0.0); + nh.param("decel_lim_theta", decel_lim_theta_, 0.0); + } + void KinematicParameters::setDirect(int *xytheta_direct) { xytheta_direct_[0] = xytheta_direct[0]; @@ -111,13 +149,11 @@ namespace mkt_plugins void KinematicParameters::setMinX(double value) { min_vel_x_ = fabs(value) < fabs(original_min_vel_x_) + EPSILON ? value : min_vel_x_; - // ROS_INFO_THROTTLE(10.0, "vx_min %f %f %f", value , original_min_vel_x_, min_vel_x_); } void KinematicParameters::setMaxX(double value) { max_vel_x_ = fabs(value) <= fabs(original_max_vel_x_) + EPSILON ? value : original_max_vel_x_; - // ROS_INFO_THROTTLE(10, "vx %f %f %f", value , original_max_vel_x_, max_vel_x_); } void KinematicParameters::setAccX(double value) diff --git a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp index 03edf58..af19d51 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp @@ -18,6 +18,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh) else { double controller_frequency = robot_nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0); + robot::log_warning("[%s:%d] limited_accel_generator: %f", __FILE__, __LINE__, controller_frequency); if (controller_frequency > 0) { acceleration_time_ = 1.0 / controller_frequency; @@ -29,6 +30,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh) acceleration_time_ = 0.05; } } + } void LimitedAccelGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity) diff --git a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp index 2820fad..6f7865f 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp @@ -14,19 +14,15 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt) { - x_it_ = std::make_shared(current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(), + robot::log_info("[%s:%d] xy_theta_iterator: %f %f %f %f", __FILE__, __LINE__, current_velocity.x, current_velocity.y, current_velocity.theta, dt); + x_it_ = std::make_shared("x_it_", current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(), kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_); - x_it_->name_ = std::string("x_it_"); - y_it_ = std::make_shared(current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(), + y_it_ = std::make_shared("y_it_", current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(), kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_); - - y_it_->name_ = std::string("y_it_"); - th_it_ = std::make_shared(current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(), + th_it_ = std::make_shared("th_it_", current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(), kinematics_->getAccTheta(), kinematics_->getDecelTheta(), dt, vtheta_samples_); - - th_it_->name_ = std::string("th_it_"); } bool XYThetaIterator::hasMoreTwists() diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner index a2bebb5..49afcce 160000 --- a/src/Algorithms/Packages/global_planners/custom_planner +++ b/src/Algorithms/Packages/global_planners/custom_planner @@ -1 +1 @@ -Subproject commit a2bebb5be969471a135601da78866cc9d1db84b1 +Subproject commit 49afcce5b2247793282fe5e94d0e27f062562325 diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h index 9a4f0e6..370e223 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h @@ -46,6 +46,12 @@ namespace pnkx_local_planner */ void setPlan(const robot_nav_2d_msgs::Path2D &path) override; + /** + * @brief robot_nav_core2 getPlan - Gets the current global plan + * @param path The global plan + */ + void getPlan(robot_nav_2d_msgs::Path2D &path) override; + /** * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity * @@ -179,9 +185,10 @@ namespace pnkx_local_planner std::string name_; robot::NodeHandle parent_, planner_nh_; robot_nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan - robot_nav_2d_msgs::Path2D transformed_plan_; + robot_nav_2d_msgs::Path2D transformed_global_plan_; + robot_nav_2d_msgs::Path2D local_plan_; robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose - + robot_costmap_2d::Costmap2D *costmap_; robot_costmap_2d::Costmap2DROBOT *costmap_robot_; nav_grid::NavGridInfo info_; 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 c4ad108..846f693 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 @@ -180,7 +180,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_ try { - if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) + if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_)) throw robot_nav_core2::LocalPlannerException("Transform global plan is failed"); } catch(const robot_nav_core2::LocalPlannerException& e) @@ -189,7 +189,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_ } double x_direction, y_direction, theta_direction; - if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) { throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); } @@ -206,9 +206,16 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner robot_nav_2d_msgs::Twist2D twist; mkt_msgs::Trajectory2D traj; robot_nav_2d_msgs::Twist2DStamped cmd_vel; - if (!ret_nav_) - traj = nav_algorithm_->calculator(pose, velocity); + if (ret_nav_) + { + local_plan_.poses.clear(); + return cmd_vel; + } + traj = nav_algorithm_->calculator(pose, velocity); + local_plan_.header.stamp = robot::Time::now(); + robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now()); + local_plan_ = robot_nav_2d_utils::pathToPath(path); cmd_vel.velocity = traj.velocity; return cmd_vel; } @@ -222,7 +229,7 @@ bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const robot_n } // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; - ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_plan_, velocity); + ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_global_plan_, velocity); if (ret_nav_) robot::log_info_at(__FILE__, __LINE__, "Goal reached!"); 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 6749fbc..120494f 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 @@ -208,6 +208,15 @@ void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path } } +void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &path) +{ + if (local_plan_.poses.empty()) + { + return; + } + path = local_plan_; +} + void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { this->getParams(planner_nh_); @@ -231,7 +240,8 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose local_goal_pose = this->transformPoseToLocal(goal_pose_); // robot::log_info("local_start_pose: %f %f %f", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta); // robot::log_info("local_goal_pose: %f %f %f", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta); - if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) + if (!pnkx_local_planner::transformGlobalPlan( + tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_)) { robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed"); throw robot_nav_core2::LocalPlannerException("Transform global plan is failed"); @@ -240,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose double x_direction, y_direction, theta_direction; if (!ret_nav_) { - if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) { robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); @@ -248,7 +258,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose } else { - if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) { robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str()); throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); @@ -291,11 +301,24 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg cmd_vel.header.stamp = robot::Time::now(); if (ret_nav_ && ret_angle_) + { return cmd_vel; + local_plan_.poses.clear(); + } else if (!ret_nav_) + { traj = nav_algorithm_->calculator(pose, velocity); + local_plan_.header.stamp = robot::Time::now(); + robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now()); + local_plan_ = robot_nav_2d_utils::pathToPath(path); + } else + { traj = rotate_algorithm_->calculator(pose, velocity); + local_plan_.header.stamp = robot::Time::now(); + robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now()); + local_plan_ = robot_nav_2d_utils::pathToPath(path); + } cmd_vel.velocity = traj.velocity; return cmd_vel; @@ -404,7 +427,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const robot_nav_2d_msgs // goal_pose_.header.stamp = pose.header.stamp; robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_); - robot_nav_2d_msgs::Path2D plan = transformed_plan_; + robot_nav_2d_msgs::Path2D plan = transformed_global_plan_; if (!ret_nav_) { ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity); 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 8b728d6..6e28f67 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 @@ -155,7 +155,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs try { - if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) + if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_)) throw robot_nav_core2::LocalPlannerException("Transform global plan is failed"); } catch(const robot_nav_core2::LocalPlannerException& e) @@ -164,7 +164,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs } double x_direction, y_direction, theta_direction; - if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) { throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); } @@ -199,10 +199,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::Sc robot_nav_2d_msgs::Twist2D twist; mkt_msgs::Trajectory2D traj; robot_nav_2d_msgs::Twist2DStamped cmd_vel; + cmd_vel.header.stamp = robot::Time::now(); if (ret_nav_) + { + local_plan_.poses.clear(); return cmd_vel; + } traj = nav_algorithm_->calculator(pose, velocity); + local_plan_.header.stamp = robot::Time::now(); + robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now()); + local_plan_ = robot_nav_2d_utils::pathToPath(path); cmd_vel.velocity = traj.velocity; return cmd_vel; } diff --git a/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h b/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h index 21571a0..871696e 100644 --- a/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h +++ b/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h @@ -159,20 +159,20 @@ protected: */ TF3SIMD_FORCE_INLINE void setMax(const QuadWord& other) { - // tf3SetMax(m_floats[0], other.m_floats[0]); - // tf3SetMax(m_floats[1], other.m_floats[1]); - // tf3SetMax(m_floats[2], other.m_floats[2]); - // tf3SetMax(m_floats[3], other.m_floats[3]); + tf3SetMax(m_floats[0], other.m_floats[0]); + tf3SetMax(m_floats[1], other.m_floats[1]); + tf3SetMax(m_floats[2], other.m_floats[2]); + tf3SetMax(m_floats[3], other.m_floats[3]); } /**@brief Set each element to the min of the current values and the values of another QuadWord * @param other The other QuadWord to compare with */ TF3SIMD_FORCE_INLINE void setMin(const QuadWord& other) { - // tf3SetMin(m_floats[0], other.m_floats[0]); - // tf3SetMin(m_floats[1], other.m_floats[1]); - // tf3SetMin(m_floats[2], other.m_floats[2]); - // tf3SetMin(m_floats[3], other.m_floats[3]); + tf3SetMin(m_floats[0], other.m_floats[0]); + tf3SetMin(m_floats[1], other.m_floats[1]); + tf3SetMin(m_floats[2], other.m_floats[2]); + tf3SetMin(m_floats[3], other.m_floats[3]); } diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index 1813d1d..824fc2f 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -27,495 +27,494 @@ #include #include - namespace robot { -namespace move_base_core -{ - // Navigation states, including planning and controller status - enum class State + namespace move_base_core { - PENDING, - ACTIVE, - PREEMPTED, - SUCCEEDED, - ABORTED, - REJECTED, - PREEMPTING, - RECALLING, - RECALLED, - LOST, - PLANNING, - CONTROLLING, - CLEARING, - PAUSED - }; - - /** - * @brief Feedback structure to describe current navigation status - */ - struct NavFeedback - { - State navigation_state; // Current navigation state - std::string feed_back_str; // Description or debug message - robot_geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D - bool goal_checked; // Whether the current goal is verified - bool is_ready; // Robot is ready for commands - }; - - /** - * @brief Planner data output structure. - */ - struct PlannerDataOutput - { - robot_nav_2d_msgs::Path2D plan; - robot_nav_msgs::OccupancyGrid costmap; - robot_map_msgs::OccupancyGridUpdate costmap_update; - bool is_costmap_updated; - robot_geometry_msgs::PolygonStamped footprint; - }; - - /** - * @brief Convert a State enum to its string representation. - * Useful for debugging/logging or displaying in UI. - * - * @param state Enum value of move_base_core::State - * @return std::string The corresponding string, e.g. "PENDING" - */ - inline std::string toString(move_base_core::State state) - { - using move_base_core::State; - switch (state) + // Navigation states, including planning and controller status + enum class State { - case State::PENDING: - return "PENDING"; // Chờ xử lý - case State::ACTIVE: - return "ACTIVE"; // Đang hoạt động - case State::PREEMPTED: - return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới - case State::SUCCEEDED: - return "SUCCEEDED"; // Thành công - case State::ABORTED: - return "ABORTED"; // Bị lỗi - case State::REJECTED: - return "REJECTED"; // Từ chối bởi planner hoặc controller - case State::PREEMPTING: - return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu - case State::RECALLING: - return "RECALLING"; // Đang huỷ bỏ nội bộ - case State::RECALLED: - return "RECALLED"; // Đã được thu hồi - case State::LOST: - return "LOST"; // Mất trạng thái - case State::PLANNING: - return "PLANNING"; // Đang lập kế hoạch đường đi - case State::CONTROLLING: - return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan - case State::CLEARING: - return "CLEARING"; // Đang dọn dẹp bản đồ / costmap - case State::PAUSED: - return "PAUSED"; // Tạm dừng - default: - return "UNKNOWN_STATE"; // Không xác định + PENDING, + ACTIVE, + PREEMPTED, + SUCCEEDED, + ABORTED, + REJECTED, + PREEMPTING, + RECALLING, + RECALLED, + LOST, + PLANNING, + CONTROLLING, + CLEARING, + PAUSED + }; + + /** + * @brief Feedback structure to describe current navigation status + */ + struct NavFeedback + { + State navigation_state; // Current navigation state + std::string feed_back_str; // Description or debug message + robot_geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D + bool goal_checked; // Whether the current goal is verified + bool is_ready; // Robot is ready for commands + }; + + /** + * @brief Planner data output structure. + */ + struct PlannerDataOutput + { + robot_nav_2d_msgs::Path2D plan; + robot_nav_msgs::OccupancyGrid costmap; + robot_map_msgs::OccupancyGridUpdate costmap_update; + bool is_costmap_updated; + robot_geometry_msgs::PolygonStamped footprint; + }; + + /** + * @brief Convert a State enum to its string representation. + * Useful for debugging/logging or displaying in UI. + * + * @param state Enum value of move_base_core::State + * @return std::string The corresponding string, e.g. "PENDING" + */ + inline std::string toString(move_base_core::State state) + { + using move_base_core::State; + switch (state) + { + case State::PENDING: + return "PENDING"; // Chờ xử lý + case State::ACTIVE: + return "ACTIVE"; // Đang hoạt động + case State::PREEMPTED: + return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới + case State::SUCCEEDED: + return "SUCCEEDED"; // Thành công + case State::ABORTED: + return "ABORTED"; // Bị lỗi + case State::REJECTED: + return "REJECTED"; // Từ chối bởi planner hoặc controller + case State::PREEMPTING: + return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu + case State::RECALLING: + return "RECALLING"; // Đang huỷ bỏ nội bộ + case State::RECALLED: + return "RECALLED"; // Đã được thu hồi + case State::LOST: + return "LOST"; // Mất trạng thái + case State::PLANNING: + return "PLANNING"; // Đang lập kế hoạch đường đi + case State::CONTROLLING: + return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan + case State::CLEARING: + return "CLEARING"; // Đang dọn dẹp bản đồ / costmap + case State::PAUSED: + return "PAUSED"; // Tạm dừng + default: + return "UNKNOWN_STATE"; // Không xác định + } } - } - - /** - * @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction. - * - * This function calculates a new position by moving forward (or backward if negative) - * a certain `offset_distance` from the current position, following the given heading angle (theta). - * - * @param pose The original 2D pose (x, y, theta) in the local plane. - * @param frame_id The coordinate frame in which the output PoseStamped will be expressed. - * @param offset_distance The distance to offset along the heading direction, in meters. - * Positive moves forward, negative moves backward. - * @return A new PoseStamped offset from the input pose, in the given frame. - */ - inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance) - { - robot_geometry_msgs::PoseStamped goal; - goal.header.frame_id = frame_id; - goal.header.stamp = robot::Time::now(); - goal.pose.position.x = pose.x + offset_distance * cos(pose.theta); - goal.pose.position.y = pose.y + offset_distance * sin(pose.theta); - - tf3::Quaternion q; - q.setRPY(0, 0, pose.theta); - goal.pose.orientation = tf3::toMsg(q); - return goal; - } - - /** - * @brief Overloaded version: creates an offset target pose from a given PoseStamped. - * - * This function extracts the 2D position and orientation (yaw) from the given PoseStamped, - * offsets it forward (or backward) along the current heading direction, - * and returns a new PoseStamped in the same frame. - * - * @param pose The original pose with full position and orientation. - * @param offset_distance Distance to offset along the current heading direction (in meters). - * @return A new PoseStamped offset from the original pose. - */ - inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance) - { - robot_geometry_msgs::Pose2D pose2d; - pose2d.x = pose.pose.position.x; - pose2d.y = pose.pose.position.y; - // pose2d.theta = tf2::getYaw(pose.pose.orientation); - return offset_goal(pose2d, pose.header.frame_id, offset_distance); - } - - /** - * @class BaseNavigation - * @brief Abstract interface for robot navigation modules. - * - * Provides core methods for setting goals, moving, rotating, and handling motion control. - * All navigation logic must implement this interface. - */ - class BaseNavigation - { - public: - using Ptr = std::shared_ptr; - - virtual ~BaseNavigation() {} /** - * @brief Initialize the navigation system. - * @param tf Shared pointer to the TF listener or buffer. - */ - virtual void initialize(TFListenerPtr tf) = 0; - - /** - * @brief Set the robot's footprint (outline shape) in the global frame. - * This can be used for planning or collision checking. + * @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction. * - * @param fprt A vector of points representing the robot's footprint polygon. - * The points should be ordered counter-clockwise. - * Example: + * This function calculates a new position by moving forward (or backward if negative) + * a certain `offset_distance` from the current position, following the given heading angle (theta). * - ^ Y - | - | P3(-0.3, 0.2) P2(0.3, 0.2) - | ●---------------● - | | | - | | Robot | (view Top ) - | | | - | ●---------------● - | P4(-0.3, -0.2) P1(0.3, -0.2) - +-------------------------------> X - - std::vector footprint; - 1. footprint.push_back(make_point(0.3, -0.2)); - 2. footprint.push_back(make_point(0.3, 0.2)); - 3. footprint.push_back(make_point(-0.3, 0.2)); - 4. footprint.push_back(make_point(-0.3, -0.2)); + * @param pose The original 2D pose (x, y, theta) in the local plane. + * @param frame_id The coordinate frame in which the output PoseStamped will be expressed. + * @param offset_distance The distance to offset along the heading direction, in meters. + * Positive moves forward, negative moves backward. + * @return A new PoseStamped offset from the input pose, in the given frame. */ - virtual void setRobotFootprint(const std::vector &fprt) = 0; + inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance) + { + robot_geometry_msgs::PoseStamped goal; + goal.header.frame_id = frame_id; + goal.header.stamp = robot::Time::now(); + goal.pose.position.x = pose.x + offset_distance * cos(pose.theta); + goal.pose.position.y = pose.y + offset_distance * sin(pose.theta); + + tf3::Quaternion q; + q.setRPY(0, 0, pose.theta); + goal.pose.orientation = tf3::toMsg(q); + return goal; + } /** - * @brief Get the robot's footprint (outline shape) in the global frame. - * @return A vector of points representing the robot's footprint polygon. + * @brief Overloaded version: creates an offset target pose from a given PoseStamped. + * + * This function extracts the 2D position and orientation (yaw) from the given PoseStamped, + * offsets it forward (or backward) along the current heading direction, + * and returns a new PoseStamped in the same frame. + * + * @param pose The original pose with full position and orientation. + * @param offset_distance Distance to offset along the current heading direction (in meters). + * @return A new PoseStamped offset from the original pose. */ - virtual std::vector getRobotFootprint() = 0; + inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance) + { + robot_geometry_msgs::Pose2D pose2d; + pose2d.x = pose.pose.position.x; + pose2d.y = pose.pose.position.y; + // pose2d.theta = tf2::getYaw(pose.pose.orientation); + return offset_goal(pose2d, pose.header.frame_id, offset_distance); + } /** - * @brief Add a static map to the navigation system. - * @param map_name The name of the map. - * @param map The map to add. + * @class BaseNavigation + * @brief Abstract interface for robot navigation modules. + * + * Provides core methods for setting goals, moving, rotating, and handling motion control. + * All navigation logic must implement this interface. */ - virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) = 0; + class BaseNavigation + { + public: + using Ptr = std::shared_ptr; - /** - * @brief Add a laser scan to the navigation system. - * @param laser_scan_name The name of the laser scan. - * @param laser_scan The laser scan to add. - */ - virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) = 0; - - /** - * @brief Add a point cloud to the navigation system. - * @param point_cloud_name The name of the point cloud. - * @param point_cloud The point cloud to add. - */ - virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) = 0; - - /** - * @brief Add a point cloud2 to the navigation system. - * @param point_cloud2_name The name of the point cloud2. - * @param point_cloud2 The point cloud2 to add. - */ - virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0; - - /** - * @brief Get a static map from the navigation system. - * @param map_name The name of the map. - * @return The map. - */ - virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) = 0; - - /** - * @brief Get a laser scan from the navigation system. - * @param laser_scan_name The name of the laser scan. - * @return The laser scan. - */ - virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) = 0; - - /** - * @brief Get a point cloud from the navigation system. - * @param point_cloud_name The name of the point cloud. - * @return The point cloud. - */ - virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) = 0; - - /** - * @brief Get a point cloud2 from the navigation system. - * @param point_cloud2_name The name of the point cloud2. - * @return The point cloud2. - */ - virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) = 0; - - /** - * @brief Get all static maps from the navigation system. - * @return The static maps. - */ - virtual std::map getAllStaticMaps() = 0; - - /** - * @brief Get all laser scans from the navigation system. - * @return The laser scans. - */ - virtual std::map getAllLaserScans() = 0; - - /** - * @brief Get all point clouds from the navigation system. - * @return The point clouds. - */ - virtual std::map getAllPointClouds() = 0; - - /** - * @brief Get all point cloud2s from the navigation system. - * @return The point cloud2s. - */ - virtual std::map getAllPointCloud2s() = 0; - - /** - * @brief Remove a static map from the navigation system. - * @param map_name The name of the map. - * @return True if the map was removed successfully. - */ - virtual bool removeStaticMap(const std::string &map_name) = 0; - - /** - * @brief Remove a laser scan from the navigation system. - * @param laser_scan_name The name of the laser scan. - * @return True if the laser scan was removed successfully. - */ - virtual bool removeLaserScan(const std::string &laser_scan_name) = 0; - - /** - * @brief Remove a point cloud from the navigation system. - * @param point_cloud_name The name of the point cloud. - * @return True if the point cloud was removed successfully. - */ - virtual bool removePointCloud(const std::string &point_cloud_name) = 0; - - /** - * @brief Remove a point cloud2 from the navigation system. - * @param point_cloud2_name The name of the point cloud2. - * @return True if the point cloud2 was removed successfully. - */ - virtual bool removePointCloud2(const std::string &point_cloud2_name) = 0; - - /** - * @brief Remove all static maps from the navigation system. - * @return True if the static maps were removed successfully. - */ - virtual bool removeAllStaticMaps() = 0; - - /** - * @brief Remove all laser scans from the navigation system. - * @return True if the laser scans were removed successfully. - */ - virtual bool removeAllLaserScans() = 0; - - /** - * @brief Remove all point clouds from the navigation system. - * @return True if the point clouds were removed successfully. - */ - virtual bool removeAllPointClouds() = 0; - - /** - * @brief Remove all point cloud2s from the navigation system. - * @return True if the point cloud2s were removed successfully. - */ - virtual bool removeAllPointCloud2s() = 0; - - /** - * @brief Remove all data from the navigation system. - * @return True if the data was removed successfully. - */ - virtual bool removeAllData() = 0; + virtual ~BaseNavigation() {} - /** - * @brief Add an odometry to the navigation system. - * @param odometry_name The name of the odometry. - * @param odometry The odometry to add. - */ - virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) = 0; + /** + * @brief Initialize the navigation system. + * @param tf Shared pointer to the TF listener or buffer. + */ + virtual void initialize(TFListenerPtr tf) = 0; - /** - * @brief Send a goal for the robot to navigate to. - * @param goal Target pose in the global frame. - * @param xy_goal_tolerance Acceptable error in X/Y (meters). - * @param yaw_goal_tolerance Acceptable angular error (radians). - * @return True if goal was accepted and sent successfully. - */ - virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal, - double xy_goal_tolerance = 0.0, - double yaw_goal_tolerance = 0.0) = 0; + /** + * @brief Set the robot's footprint (outline shape) in the global frame. + * This can be used for planning or collision checking. + * + * @param fprt A vector of points representing the robot's footprint polygon. + * The points should be ordered counter-clockwise. + * Example: + * + ^ Y + | + | P3(-0.3, 0.2) P2(0.3, 0.2) + | ●---------------● + | | | + | | Robot | (view Top ) + | | | + | ●---------------● + | P4(-0.3, -0.2) P1(0.3, -0.2) + +-------------------------------> X - /** - * @brief Send a goal for the robot to navigate to. - * @param msg Order message. - * @param goal Target pose in the global frame. - * @param xy_goal_tolerance Acceptable error in X/Y (meters). - * @param yaw_goal_tolerance Acceptable angular error (radians). - * @return True if goal was accepted and sent successfully. - */ - virtual bool moveTo(const robot_protocol_msgs::Order &msg, - const robot_geometry_msgs::PoseStamped &goal, - double xy_goal_tolerance = 0.0, - double yaw_goal_tolerance = 0.0) = 0; + std::vector footprint; + 1. footprint.push_back(make_point(0.3, -0.2)); + 2. footprint.push_back(make_point(0.3, 0.2)); + 3. footprint.push_back(make_point(-0.3, 0.2)); + 4. footprint.push_back(make_point(-0.3, -0.2)); + */ + virtual void setRobotFootprint(const std::vector &fprt) = 0; - /** - * @brief Send a docking goal to a predefined marker (e.g. charger). - * @param maker Marker name or ID. - * @param goal Target pose for docking. - * @param xy_goal_tolerance Acceptable XY error (meters). - * @param yaw_goal_tolerance Acceptable heading error (radians). - * @return True if docking command succeeded. - */ - virtual bool dockTo(const std::string &maker, - const robot_geometry_msgs::PoseStamped &goal, - double xy_goal_tolerance = 0.0, - double yaw_goal_tolerance = 0.0) = 0; + /** + * @brief Get the robot's footprint (outline shape) in the global frame. + * @return A vector of points representing the robot's footprint polygon. + */ + virtual std::vector getRobotFootprint() = 0; - /** - * @brief Send a docking goal to a predefined marker (e.g. charger). - * @param msg Order message. - * @param goal Target pose for docking. - * @param xy_goal_tolerance Acceptable XY error (meters). - * @param yaw_goal_tolerance Acceptable heading error (radians). - * @return True if docking command succeeded. - */ - virtual bool dockTo(const robot_protocol_msgs::Order &msg, - const robot_geometry_msgs::PoseStamped &goal, - double xy_goal_tolerance = 0.0, - double yaw_goal_tolerance = 0.0) = 0; + /** + * @brief Add a static map to the navigation system. + * @param map_name The name of the map. + * @param map The map to add. + */ + virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) = 0; - /** - * @brief Move straight toward the target position (X-axis). - * @param goal Target pose. - * @param xy_goal_tolerance Acceptable positional error (meters). - * @return True if command issued successfully. - */ - virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, - double xy_goal_tolerance = 0.0) = 0; + /** + * @brief Add a laser scan to the navigation system. + * @param laser_scan_name The name of the laser scan. + * @param laser_scan The laser scan to add. + */ + virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) = 0; - /** - * @brief Rotate in place to align with target orientation. - * @param goal Pose containing desired heading (only Z-axis used). - * @param yaw_goal_tolerance Acceptable angular error (radians). - * @return True if rotation command was sent successfully. - */ - virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal, + /** + * @brief Add a point cloud to the navigation system. + * @param point_cloud_name The name of the point cloud. + * @param point_cloud The point cloud to add. + */ + virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) = 0; + + /** + * @brief Add a point cloud2 to the navigation system. + * @param point_cloud2_name The name of the point cloud2. + * @param point_cloud2 The point cloud2 to add. + */ + virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0; + + /** + * @brief Get a static map from the navigation system. + * @param map_name The name of the map. + * @return The map. + */ + virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) = 0; + + /** + * @brief Get a laser scan from the navigation system. + * @param laser_scan_name The name of the laser scan. + * @return The laser scan. + */ + virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) = 0; + + /** + * @brief Get a point cloud from the navigation system. + * @param point_cloud_name The name of the point cloud. + * @return The point cloud. + */ + virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) = 0; + + /** + * @brief Get a point cloud2 from the navigation system. + * @param point_cloud2_name The name of the point cloud2. + * @return The point cloud2. + */ + virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) = 0; + + /** + * @brief Get all static maps from the navigation system. + * @return The static maps. + */ + virtual std::map getAllStaticMaps() = 0; + + /** + * @brief Get all laser scans from the navigation system. + * @return The laser scans. + */ + virtual std::map getAllLaserScans() = 0; + + /** + * @brief Get all point clouds from the navigation system. + * @return The point clouds. + */ + virtual std::map getAllPointClouds() = 0; + + /** + * @brief Get all point cloud2s from the navigation system. + * @return The point cloud2s. + */ + virtual std::map getAllPointCloud2s() = 0; + + /** + * @brief Remove a static map from the navigation system. + * @param map_name The name of the map. + * @return True if the map was removed successfully. + */ + virtual bool removeStaticMap(const std::string &map_name) = 0; + + /** + * @brief Remove a laser scan from the navigation system. + * @param laser_scan_name The name of the laser scan. + * @return True if the laser scan was removed successfully. + */ + virtual bool removeLaserScan(const std::string &laser_scan_name) = 0; + + /** + * @brief Remove a point cloud from the navigation system. + * @param point_cloud_name The name of the point cloud. + * @return True if the point cloud was removed successfully. + */ + virtual bool removePointCloud(const std::string &point_cloud_name) = 0; + + /** + * @brief Remove a point cloud2 from the navigation system. + * @param point_cloud2_name The name of the point cloud2. + * @return True if the point cloud2 was removed successfully. + */ + virtual bool removePointCloud2(const std::string &point_cloud2_name) = 0; + + /** + * @brief Remove all static maps from the navigation system. + * @return True if the static maps were removed successfully. + */ + virtual bool removeAllStaticMaps() = 0; + + /** + * @brief Remove all laser scans from the navigation system. + * @return True if the laser scans were removed successfully. + */ + virtual bool removeAllLaserScans() = 0; + + /** + * @brief Remove all point clouds from the navigation system. + * @return True if the point clouds were removed successfully. + */ + virtual bool removeAllPointClouds() = 0; + + /** + * @brief Remove all point cloud2s from the navigation system. + * @return True if the point cloud2s were removed successfully. + */ + virtual bool removeAllPointCloud2s() = 0; + + /** + * @brief Remove all data from the navigation system. + * @return True if the data was removed successfully. + */ + virtual bool removeAllData() = 0; + + /** + * @brief Add an odometry to the navigation system. + * @param odometry_name The name of the odometry. + * @param odometry The odometry to add. + */ + virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) = 0; + + /** + * @brief Send a goal for the robot to navigate to. + * @param goal Target pose in the global frame. + * @param xy_goal_tolerance Acceptable error in X/Y (meters). + * @param yaw_goal_tolerance Acceptable angular error (radians). + * @return True if goal was accepted and sent successfully. + */ + virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) = 0; - /** - * @brief Pause the robot's movement (e.g. during obstacle avoidance). - */ - virtual void pause() = 0; + /** + * @brief Send a goal for the robot to navigate to. + * @param msg Order message. + * @param goal Target pose in the global frame. + * @param xy_goal_tolerance Acceptable error in X/Y (meters). + * @param yaw_goal_tolerance Acceptable angular error (radians). + * @return True if goal was accepted and sent successfully. + */ + virtual bool moveTo(const robot_protocol_msgs::Order &msg, + const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) = 0; - /** - * @brief Resume motion after a pause. - */ - virtual void resume() = 0; + /** + * @brief Send a docking goal to a predefined marker (e.g. charger). + * @param maker Marker name or ID. + * @param goal Target pose for docking. + * @param xy_goal_tolerance Acceptable XY error (meters). + * @param yaw_goal_tolerance Acceptable heading error (radians). + * @return True if docking command succeeded. + */ + virtual bool dockTo(const std::string &maker, + const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) = 0; - /** - * @brief Cancel the current goal and stop the robot. - */ - virtual void cancel() = 0; + /** + * @brief Send a docking goal to a predefined marker (e.g. charger). + * @param msg Order message. + * @param goal Target pose for docking. + * @param xy_goal_tolerance Acceptable XY error (meters). + * @param yaw_goal_tolerance Acceptable heading error (radians). + * @return True if docking command succeeded. + */ + virtual bool dockTo(const robot_protocol_msgs::Order &msg, + const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) = 0; - /** - * @brief Send limited linear velocity command. - * @param linear Linear velocity vector (x, y, z). - * @return True if the command was accepted. - */ - virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0; + /** + * @brief Move straight toward the target position (X-axis). + * @param goal Target pose. + * @param xy_goal_tolerance Acceptable positional error (meters). + * @return True if command issued successfully. + */ + virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0) = 0; - /** - * @brief Send limited angular velocity command. - * @param angular Angular velocity vector (x, y, z). - * @return True if the command was accepted. - */ - virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) = 0; + /** + * @brief Rotate in place to align with target orientation. + * @param goal Pose containing desired heading (only Z-axis used). + * @param yaw_goal_tolerance Acceptable angular error (radians). + * @return True if rotation command was sent successfully. + */ + virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal, + double yaw_goal_tolerance = 0.0) = 0; - /** - * @brief Get the robot’s pose as a PoseStamped. - * @param pose Output parameter with the robot’s current pose. - * @return True if pose was successfully retrieved. - */ - virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) = 0; + /** + * @brief Pause the robot's movement (e.g. during obstacle avoidance). + */ + virtual void pause() = 0; - /** - * @brief Get the robot’s pose as a 2D pose. - * @param pose Output parameter with the robot’s current 2D pose. - * @return True if pose was successfully retrieved. - */ - virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) = 0; + /** + * @brief Resume motion after a pause. + */ + virtual void resume() = 0; - /** - * @brief Get the robot’s twist. - * @return The robot’s current twist. - */ - virtual robot_nav_2d_msgs::Twist2DStamped getTwist() = 0; + /** + * @brief Cancel the current goal and stop the robot. + */ + virtual void cancel() = 0; - /** - * @brief Get the navigation feedback. - * @return Pointer to the navigation feedback. - */ - virtual NavFeedback *getFeedback() = 0; + /** + * @brief Send limited linear velocity command. + * @param linear Linear velocity vector (x, y, z). + * @return True if the command was accepted. + */ + virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0; - /** - * @brief Get the global data. - * @return The global data. - */ - virtual PlannerDataOutput getGlobalData() = 0; + /** + * @brief Send limited angular velocity command. + * @param angular Angular velocity vector (x, y, z). + * @return True if the command was accepted. + */ + virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) = 0; - /** - * @brief Get the local data. - * @return The local data. - */ - virtual PlannerDataOutput getLocalData() = 0; - - protected: - // Shared feedback data for navigation status tracking - std::shared_ptr nav_feedback_; - robot_nav_2d_msgs::Twist2DStamped twist_; - robot_nav_msgs::Odometry odometry_; - PlannerDataOutput global_data_; - PlannerDataOutput local_data_; + /** + * @brief Get the robot’s pose as a PoseStamped. + * @param pose Output parameter with the robot’s current pose. + * @return True if pose was successfully retrieved. + */ + virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) = 0; - std::map static_maps_; - std::map laser_scans_; - std::map point_clouds_; - std::map point_cloud2s_; + /** + * @brief Get the robot’s pose as a 2D pose. + * @param pose Output parameter with the robot’s current 2D pose. + * @return True if pose was successfully retrieved. + */ + virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) = 0; - BaseNavigation() = default; - }; + /** + * @brief Get the robot’s twist. + * @return The robot’s current twist. + */ + virtual robot_nav_2d_msgs::Twist2DStamped getTwist() = 0; -} // namespace move_base_core + /** + * @brief Get the navigation feedback. + * @return Pointer to the navigation feedback. + */ + virtual NavFeedback *getFeedback() = 0; + + /** + * @brief Get the global data. + * @return The global data. + */ + virtual PlannerDataOutput getGlobalData() = 0; + + /** + * @brief Get the local data. + * @return The local data. + */ + virtual PlannerDataOutput getLocalData() = 0; + + protected: + // Shared feedback data for navigation status tracking + std::shared_ptr nav_feedback_; + robot_nav_2d_msgs::Twist2DStamped twist_; + robot_nav_msgs::Odometry odometry_; + PlannerDataOutput global_data_; + PlannerDataOutput local_data_; + + std::map static_maps_; + std::map laser_scans_; + std::map point_clouds_; + std::map point_cloud2s_; + + BaseNavigation() = default; + }; + + } // namespace move_base_core }; #endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_ 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 566cb2e..5373293 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 @@ -133,6 +133,13 @@ namespace robot_nav_core */ virtual bool setPlan(const std::vector &plan) = 0; + /** + * @brief Gets the global plan for this local planner. + * + * @param path The global plan + */ + virtual void getPlan(std::vector &path) = 0; + /** * @brief Constructs the local planner * @param name The name to give this instance of the local planner diff --git a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h index 9adea92..95527ae 100755 --- a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h @@ -92,6 +92,13 @@ namespace robot_nav_core2 */ virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0; + /** + * @brief Gets the global plan for this local planner. + * + * @param path The global plan + */ + virtual void getPlan(robot_nav_2d_msgs::Path2D &path) = 0; + /** * @brief Compute the best command given the current pose, velocity and goal * 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 eb47b44..839f198 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 @@ -159,6 +159,14 @@ namespace robot_nav_core_adapter */ bool setPlan(const std::vector &plan) override; + + /** + * @brief Gets the global plan for this local planner. + * + * @param path The global plan + */ + virtual void getPlan(std::vector &path) override; + /** * @brief Create a new LocalPlannerAdapter * @return A shared pointer to the new LocalPlannerAdapter 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 c4671a8..696c49e 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 @@ -366,6 +366,17 @@ namespace robot_nav_core_adapter } } + void LocalPlannerAdapter::getPlan(std::vector &path) + { + if (!planner_) + { + return; + } + robot_nav_2d_msgs::Path2D path2d; + planner_->getPlan(path2d); + path = robot_nav_2d_utils::pathToPath(path2d).poses; + } + bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal) { if (last_goal_.header.frame_id != new_goal.header.frame_id || diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 91ef2b5..f4725da 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -607,7 +607,6 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) { - robot::log_info("[%s:%d] addOdometry called for: %s", __FILE__, __LINE__, odometry_name.c_str()); odometry_ = odometry; } @@ -2206,6 +2205,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) { if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel)) { + robot_nav_msgs::Path path; + tc_->getPlan(path.poses); + if (!path.poses.empty()) + { + path.header.stamp = path.poses[0].header.stamp; + path.header.frame_id = path.poses[0].header.frame_id; + } + else + { + path.header.stamp = robot::Time::now(); + path.header.frame_id = controller_costmap_robot_->getGlobalFrameID(); + } + local_data_.plan = robot_nav_2d_utils::pathToPath(path); + last_valid_control_ = robot::Time::now(); // make sure that we send the velocity command to the base @@ -2283,6 +2296,9 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) lock.unlock(); } } + twist_.velocity = robot_nav_2d_utils::twist3Dto2D(cmd_vel); + twist_.header.stamp = robot::Time::now(); + twist_.header.frame_id = controller_costmap_robot_->getGlobalFrameID(); } else {