update 28/2
This commit is contained in:
parent
575e190988
commit
620db96de0
|
|
@ -1,4 +1,3 @@
|
|||
|
||||
LocalPlannerAdapter:
|
||||
library_path: liblocal_planner_adapter
|
||||
yaw_goal_tolerance: 0.017
|
||||
|
|
@ -77,7 +76,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
|
|||
library_path: libmkt_algorithm_diff
|
||||
avoid_obstacles: false
|
||||
xy_local_goal_tolerance: 0.01
|
||||
angle_threshold: 0.6
|
||||
angle_threshold: 0.4
|
||||
index_samples: 60
|
||||
follow_step_path: true
|
||||
|
||||
|
|
@ -88,9 +87,10 @@ MKTAlgorithmDiffPredictiveTrajectory:
|
|||
# only when true:
|
||||
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)
|
||||
lookahead_time: 1.9 # 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)
|
||||
max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2)
|
||||
max_lateral_accel: 2.0 # Max lateral accel for speed reduction on curves (m/s^2)
|
||||
|
||||
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
|
||||
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
|
||||
|
|
@ -141,9 +141,9 @@ 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: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3)
|
||||
min_lookahead_dist: 0.325 # 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)
|
||||
lookahead_time: 1.9 # 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)
|
||||
max_journey_squared: 0.6 # Minimum squared journey to consider for goal (default: 0.2)
|
||||
|
||||
|
|
|
|||
|
|
@ -81,7 +81,7 @@ endif()
|
|||
# Libraries
|
||||
# ========================================================
|
||||
add_library(${PROJECT_NAME}_diff SHARED
|
||||
src/diff/diff_predictive_trajectory.cpp
|
||||
src/diff/diff_predictive_trajectory_.cpp
|
||||
src/diff/diff_rotate_to_goal.cpp
|
||||
src/diff/diff_go_straight.cpp
|
||||
# src/diff/pure_pursuit.cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,314 @@
|
|||
#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
|
||||
#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
|
||||
|
||||
#include <robot/robot.h>
|
||||
#include <score_algorithm/score_algorithm.h>
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/PointStamped.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <robot_nav_core2/exceptions.h>
|
||||
#include <robot_nav_core2/costmap.h>
|
||||
#include <nav_grid/coordinate_conversion.h>
|
||||
#include <angles/angles.h>
|
||||
#include <robot_nav_msgs/Path.h>
|
||||
#include <kalman/kalman.h>
|
||||
#include <vector>
|
||||
|
||||
#include <robot_nav_2d_utils/parameters.h>
|
||||
#include <robot_nav_2d_utils/tf_help.h>
|
||||
#include <robot_nav_2d_utils/path_ops.h>
|
||||
#include <robot_nav_2d_utils/conversions.h>
|
||||
|
||||
namespace mkt_algorithm
|
||||
{
|
||||
namespace diff
|
||||
{
|
||||
/**
|
||||
* @class PredictiveTrajectory
|
||||
* @brief Standard PredictiveTrajectory-like ScoreAlgorithm
|
||||
*/
|
||||
class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
|
||||
{
|
||||
public:
|
||||
PredictiveTrajectory() : initialized_(false), nav_stop_(false) {};
|
||||
|
||||
virtual ~PredictiveTrajectory();
|
||||
|
||||
// Standard ScoreAlgorithm Interface
|
||||
/**
|
||||
* @brief Initialize parameters as needed
|
||||
* @param nh NodeHandle to read parameters from
|
||||
*/
|
||||
virtual void initialize(
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
|
||||
|
||||
/**
|
||||
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
|
||||
*
|
||||
* Subclasses may overwrite. Return false in case there is any error.
|
||||
*
|
||||
* @param pose Current pose (costmap frame)
|
||||
* @param velocity Current velocity
|
||||
* @param goal The final goal (costmap frame)
|
||||
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
|
||||
*/
|
||||
virtual bool 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) override;
|
||||
|
||||
/**
|
||||
* @brief Calculating algorithm
|
||||
* @param pose
|
||||
* @param velocity
|
||||
* @param traj
|
||||
*/
|
||||
virtual mkt_msgs::Trajectory2D calculator(
|
||||
const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;
|
||||
|
||||
/**
|
||||
* @brief Reset all data
|
||||
*/
|
||||
virtual void reset() override;
|
||||
|
||||
/**
|
||||
* @brief Stoping move navigation
|
||||
*/
|
||||
virtual void stop() override;
|
||||
|
||||
/**
|
||||
* @brief resume move navigation after stopped
|
||||
*/
|
||||
virtual void resume() override;
|
||||
|
||||
/**
|
||||
* @brief Create a new PredictiveTrajectory instance
|
||||
* @return A pointer to the new PredictiveTrajectory instance
|
||||
*/
|
||||
static score_algorithm::ScoreAlgorithm::Ptr create();
|
||||
|
||||
protected:
|
||||
inline double sign(double x)
|
||||
{
|
||||
return x < 0.0 ? -1.0 : 1.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialize parameters
|
||||
*/
|
||||
virtual void getParams();
|
||||
|
||||
/**
|
||||
* @brief Dynamically adjust look ahead distance based on the speed
|
||||
* @param velocity
|
||||
* @return look ahead distance
|
||||
*/
|
||||
double getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity);
|
||||
|
||||
/**
|
||||
* @brief Get lookahead point on the global plan
|
||||
* @param lookahead_dist
|
||||
* @param global_plan
|
||||
* @return lookahead point
|
||||
*/
|
||||
std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
|
||||
getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);
|
||||
|
||||
/**
|
||||
* @brief Prune global plan
|
||||
* @param tf
|
||||
* @param pose
|
||||
* @param global_plan
|
||||
* @param dist_behind_robot
|
||||
* @return true if plan is pruned, false otherwise
|
||||
*/
|
||||
bool pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot);
|
||||
|
||||
/**
|
||||
* @brief Transforms the global plan of the robot from the planner frame to the local frame (modified).
|
||||
*
|
||||
* The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
|
||||
* such that the index of the current goal pose is returned as well as
|
||||
* the transformation between the global plan and the planning frame.
|
||||
* @param tf A reference to a tf buffer
|
||||
* @param global_plan The plan to be transformed
|
||||
* @param pose The pose of the robot
|
||||
* @param costmap A reference to the costmap being used so the window size for transforming can be computed
|
||||
* @param global_frame The frame to transform the plan to
|
||||
* @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
|
||||
* @param[out] transformed_plan Populated with the transformed plan
|
||||
* @return \c true if the global plan is transformed, \c false otherwise
|
||||
*/
|
||||
bool 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);
|
||||
|
||||
robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose);
|
||||
|
||||
/**
|
||||
* @brief Should rotate to path
|
||||
* @param carrot_pose
|
||||
* @param angle_to_path
|
||||
* @return true if should rotate, false otherwise
|
||||
*/
|
||||
bool 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);
|
||||
|
||||
/**
|
||||
* @brief Rotate to heading
|
||||
* @param angle_to_path
|
||||
* @param velocity The velocity of the robot
|
||||
* @param cmd_vel The velocity commands to be filled
|
||||
*/
|
||||
void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
|
||||
|
||||
/**
|
||||
* @brief the robot is moving acceleration limits
|
||||
* @param velocity The velocity of the robot
|
||||
* @param cmd_vel The velocity commands
|
||||
* @param cmd_vel_result The velocity commands result
|
||||
*/
|
||||
void moveWithAccLimits(
|
||||
const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result);
|
||||
|
||||
/**
|
||||
* @brief Stop the robot taking into account acceleration limits
|
||||
* @param pose The pose of the robot in the global frame
|
||||
* @param velocity The velocity of the robot
|
||||
* @param cmd_vel The velocity commands to be filled
|
||||
* @return True if a valid trajectory was found, false otherwise
|
||||
*/
|
||||
bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
|
||||
|
||||
/**
|
||||
* @brief Apply constraints
|
||||
* @param dist_error
|
||||
* @param lookahead_dist
|
||||
* @param curvature
|
||||
* @param curr_speed
|
||||
* @param pose_cost
|
||||
* @param linear_vel
|
||||
* @param sign
|
||||
*/
|
||||
void applyConstraints(
|
||||
const double &dist_error, const double &lookahead_dist,
|
||||
const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
|
||||
const double &pose_cost, double &linear_vel, const double &sign_x);
|
||||
|
||||
void computePurePursuit(
|
||||
const score_algorithm::TrajectoryGenerator::Ptr &traj,
|
||||
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
|
||||
const robot_nav_2d_msgs::Twist2D &velocity,
|
||||
const double &min_approach_linear_velocity,
|
||||
const double &journey_plan,
|
||||
const double &sign_x,
|
||||
const double &lookahead_dist_min,
|
||||
const double &lookahead_dist_max,
|
||||
const double &lookahead_dist,
|
||||
const double &lookahead_time,
|
||||
const double &dt,
|
||||
robot_nav_2d_msgs::Twist2D &drive_cmd
|
||||
);
|
||||
|
||||
double adjustSpeedWithHermiteTrajectory(
|
||||
const robot_nav_2d_msgs::Twist2D &velocity,
|
||||
const robot_nav_2d_msgs::Path2D &trajectory,
|
||||
double v_target,
|
||||
const double &sign_x
|
||||
);
|
||||
|
||||
std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
|
||||
|
||||
/**
|
||||
* @brief Cost at pose
|
||||
* @param x
|
||||
* @param y
|
||||
* @return cost
|
||||
*/
|
||||
double costAtPose(const double &x, const double &y);
|
||||
|
||||
double computeDecelerationFactor(double remaining_distance, double decel_distance);
|
||||
|
||||
double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);
|
||||
|
||||
double estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan);
|
||||
|
||||
|
||||
std::vector<double> angle_history_;
|
||||
size_t window_size_;
|
||||
|
||||
bool initialized_;
|
||||
bool nav_stop_;
|
||||
robot::NodeHandle nh_, nh_priv_;
|
||||
std::string frame_id_path_;
|
||||
std::string robot_base_frame_;
|
||||
|
||||
robot_nav_2d_msgs::Pose2DStamped goal_;
|
||||
robot_nav_2d_msgs::Path2D global_plan_;
|
||||
robot_nav_2d_msgs::Path2D compute_plan_;
|
||||
robot_nav_2d_msgs::Path2D transform_plan_;
|
||||
robot_nav_2d_msgs::Twist2D prevous_drive_cmd_;
|
||||
|
||||
double x_direction_, y_direction_, theta_direction_;
|
||||
double max_robot_pose_search_dist_;
|
||||
double global_plan_prune_distance_{1.0};
|
||||
|
||||
// Lookahead
|
||||
bool use_velocity_scaled_lookahead_dist_;
|
||||
double lookahead_time_;
|
||||
double lookahead_dist_;
|
||||
double min_lookahead_dist_;
|
||||
double max_lookahead_dist_;
|
||||
double max_lateral_accel_;
|
||||
|
||||
// journey
|
||||
double min_journey_squared_{1e9};
|
||||
double max_journey_squared_{1e9};
|
||||
|
||||
// Rotate to heading
|
||||
bool use_rotate_to_heading_;
|
||||
double rotate_to_heading_min_angle_;
|
||||
|
||||
double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
|
||||
double min_path_distance_, max_path_distance_;
|
||||
|
||||
// Regulated linear velocity scaling
|
||||
bool use_regulated_linear_velocity_scaling_;
|
||||
double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
|
||||
double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
|
||||
|
||||
double rot_stopped_velocity_, trans_stopped_velocity_;
|
||||
|
||||
double min_approach_linear_velocity_;
|
||||
double regulated_linear_scaling_min_radius_;
|
||||
double regulated_linear_scaling_min_speed_;
|
||||
|
||||
bool use_cost_regulated_linear_velocity_scaling_;
|
||||
double inflation_cost_scaling_factor_;
|
||||
double cost_scaling_dist_, cost_scaling_gain_;
|
||||
|
||||
double cost_left_goal_, cost_right_goal_;
|
||||
double cost_left_side_ , cost_right_side_;
|
||||
double center_cost_;
|
||||
|
||||
// Control frequency
|
||||
double control_duration_;
|
||||
std::vector<robot_geometry_msgs::Point> footprint_spec_;
|
||||
|
||||
robot::Time last_actuator_update_;
|
||||
boost::shared_ptr<KalmanFilter> kf_;
|
||||
int m_, n_;
|
||||
Eigen::MatrixXd A;
|
||||
Eigen::MatrixXd C;
|
||||
Eigen::MatrixXd Q;
|
||||
Eigen::MatrixXd R;
|
||||
Eigen::MatrixXd P;
|
||||
|
||||
}; // class PredictiveTrajectory
|
||||
|
||||
} // namespace diff
|
||||
|
||||
} // namespace mkt_algorithm
|
||||
|
||||
#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
|
||||
|
|
@ -3,8 +3,6 @@
|
|||
|
||||
#include <robot/robot.h>
|
||||
#include <score_algorithm/score_algorithm.h>
|
||||
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/PointStamped.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
|
|
@ -17,11 +15,9 @@ namespace mkt_algorithm
|
|||
{
|
||||
namespace diff
|
||||
{
|
||||
class PurePursuit : public score_algorithm::ScoreAlgorithm
|
||||
class PurePursuit
|
||||
{
|
||||
public:
|
||||
PurePursuit();
|
||||
~PurePursuit();
|
||||
void computePurePursuit(
|
||||
const score_algorithm::TrajectoryGenerator::Ptr &traj,
|
||||
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
|
||||
|
|
@ -33,6 +29,7 @@ namespace mkt_algorithm
|
|||
const double &lookahead_dist_max,
|
||||
const double &lookahead_dist,
|
||||
const double &lookahead_time,
|
||||
const double &dt,
|
||||
robot_nav_2d_msgs::Twist2D &drive_cmd
|
||||
);
|
||||
|
||||
|
|
@ -52,5 +49,4 @@ namespace mkt_algorithm
|
|||
};
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
121
src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md
Normal file
121
src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md
Normal file
|
|
@ -0,0 +1,121 @@
|
|||
## Hermite (quỹ đạo 2D) từ robot đến look-ahead pose
|
||||
|
||||
|
||||
|
||||
### Bài toán
|
||||
Cho pose đích trong hệ tọa độ robot: `goal = (x, y, theta)`, với robot ở gốc
|
||||
`(0, 0, 0)` (hướng theo trục +x). Cần sinh quỹ đạo từ robot đến pose đích.
|
||||
|
||||
### Ý tưởng
|
||||
Dùng đường cong Hermite bậc 3 để đảm bảo:
|
||||
- Đi qua đúng điểm đầu và cuối.
|
||||
- Đúng hướng đầu và cuối.
|
||||
|
||||
Đường cong tham số `t` từ `0` đến `1`:
|
||||
|
||||
```
|
||||
p(t) = (x(t), y(t))
|
||||
```
|
||||
|
||||
Ràng buộc:
|
||||
- p(0) = (0, 0), hướng 0 rad.
|
||||
- p(1) = (x, y), hướng theta rad.
|
||||
|
||||
### Hệ số Hermite
|
||||
```
|
||||
h00 = 2t^3 - 3t^2 + 1
|
||||
h10 = t^3 - 2t^2 + t
|
||||
h01 = -2t^3 + 3t^2
|
||||
h11 = t^3 - t^2
|
||||
```
|
||||
|
||||
### Đạo hàm đầu/cuối
|
||||
Chọn độ dài đặc trưng `L` (thường L = sqrt(x^2 + y^2)):
|
||||
```
|
||||
p'(0) = (L, 0)
|
||||
p'(1) = (L*cos(theta), L*sin(theta))
|
||||
```
|
||||
|
||||
### Công thức quỹ đạo
|
||||
```
|
||||
x(t) = h10*L + h01*x + h11*L*cos(theta)
|
||||
y(t) = h01*y + h11*L*sin(theta)
|
||||
```
|
||||
|
||||
### Hướng (heading) trên đường cong
|
||||
Đạo hàm:
|
||||
```
|
||||
h00' = 6t^2 - 6t
|
||||
h10' = 3t^2 - 4t + 1
|
||||
h01' = -6t^2 + 6t
|
||||
h11' = 3t^2 - 2t
|
||||
|
||||
dx = h10'*L + h01'*x + h11'*L*cos(theta)
|
||||
dy = h01'*y + h11'*L*sin(theta)
|
||||
```
|
||||
Hướng:
|
||||
```
|
||||
heading(t) = atan2(dy, dx)
|
||||
```
|
||||
|
||||
### Cách sử dụng
|
||||
Lấy mẫu `t` đều (ví dụ 50-200 điểm) để tạo danh sách điểm quỹ đạo.
|
||||
Nếu cần độ cong, có thể tính thêm từ đạo hàm bậc 2.
|
||||
|
||||
### Ví dụ code C++
|
||||
```
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
|
||||
struct Pose2D {
|
||||
double x;
|
||||
double y;
|
||||
double theta;
|
||||
};
|
||||
|
||||
std::vector<Pose2D> generateHermiteTrajectory(
|
||||
double x, double y, double theta,
|
||||
int samples = 100)
|
||||
{
|
||||
std::vector<Pose2D> path;
|
||||
path.reserve(samples + 1);
|
||||
|
||||
double L = std::sqrt(x * x + y * y);
|
||||
if (L < 1e-6) {
|
||||
path.push_back({0.0, 0.0, 0.0});
|
||||
return path;
|
||||
}
|
||||
|
||||
for (int i = 0; i <= samples; ++i) {
|
||||
double t = static_cast<double>(i) / samples;
|
||||
double t2 = t * t;
|
||||
double t3 = t2 * t;
|
||||
|
||||
double h00 = 2 * t3 - 3 * t2 + 1;
|
||||
double h10 = t3 - 2 * t2 + t;
|
||||
double h01 = -2 * t3 + 3 * t2;
|
||||
double h11 = t3 - t2;
|
||||
|
||||
double px = h10 * L + h01 * x + h11 * (L * std::cos(theta));
|
||||
double py = h01 * y + h11 * (L * std::sin(theta));
|
||||
|
||||
double dh00 = 6 * t2 - 6 * t;
|
||||
double dh10 = 3 * t2 - 4 * t + 1;
|
||||
double dh01 = -6 * t2 + 6 * t;
|
||||
double dh11 = 3 * t2 - 2 * t;
|
||||
|
||||
double dx = dh10 * L + dh01 * x + dh11 * (L * std::cos(theta));
|
||||
double dy = dh01 * y + dh11 * (L * std::sin(theta));
|
||||
|
||||
double heading = std::atan2(dy, dx);
|
||||
|
||||
path.push_back({px, py, heading});
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
```
|
||||
|
||||
### Ghi chú
|
||||
- Nếu (x, y) rất gần 0 thì quỹ đạo suy biến, chỉ trả về điểm gốc.
|
||||
- L có thể điều chỉnh lớn hơn để quỹ đạo mềm hơn.
|
||||
|
|
@ -10,7 +10,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
|
|||
if (!initialized_)
|
||||
{
|
||||
nh_ = robot::NodeHandle("~/");
|
||||
nh_priv_ = robot::NodeHandle("~/" + name);
|
||||
nh_priv_ = robot::NodeHandle(nh, name);
|
||||
name_ = name;
|
||||
tf_ = tf;
|
||||
traj_ = traj;
|
||||
|
|
@ -385,8 +385,20 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
|
|||
if (transformed_plan.poses.empty())
|
||||
{
|
||||
robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
|
||||
stopWithAccLimits(pose, velocity, drive_cmd);
|
||||
result.velocity = drive_cmd;
|
||||
prevous_drive_cmd_ = drive_cmd;
|
||||
return result;
|
||||
}
|
||||
else
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
|
|
@ -407,7 +419,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
|
|||
const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
|
||||
|
||||
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))
|
||||
{
|
||||
|
|
@ -1186,3 +1197,5 @@ score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::
|
|||
{
|
||||
return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();
|
||||
}
|
||||
|
||||
BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory)
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,40 @@
|
|||
<svg xmlns="http://www.w3.org/2000/svg" width="720" height="420" viewBox="0 0 720 420">
|
||||
<rect width="100%" height="100%" fill="white"/>
|
||||
<g stroke="#444" stroke-width="2" fill="none">
|
||||
<line x1="80" y1="340" x2="660" y2="340"/>
|
||||
<line x1="80" y1="340" x2="80" y2="60"/>
|
||||
<polygon points="660,340 648,334 648,346" fill="#444"/>
|
||||
<polygon points="80,60 74,72 86,72" fill="#444"/>
|
||||
</g>
|
||||
|
||||
<g font-family="Arial, sans-serif" font-size="14" fill="#222">
|
||||
<text x="668" y="345">x</text>
|
||||
<text x="68" y="52">y</text>
|
||||
<text x="92" y="356">O(0,0)</text>
|
||||
</g>
|
||||
|
||||
<g stroke="#1f77b4" stroke-width="3" fill="none">
|
||||
<!-- Hermite-like curve -->
|
||||
<path d="M 80 340 C 220 310, 340 240, 460 180 S 600 110, 640 120"/>
|
||||
</g>
|
||||
|
||||
<g fill="#1f77b4">
|
||||
<circle cx="80" cy="340" r="5"/>
|
||||
<circle cx="640" cy="120" r="5"/>
|
||||
</g>
|
||||
|
||||
<g stroke="#888" stroke-width="2" fill="none">
|
||||
<!-- Start heading (0 rad) -->
|
||||
<line x1="80" y1="340" x2="140" y2="340"/>
|
||||
<polygon points="140,340 130,334 130,346" fill="#888"/>
|
||||
<!-- Goal heading (theta) - aligned with curve tangent -->
|
||||
<line x1="640" y1="120" x2="690" y2="130"/>
|
||||
<polygon points="690,130 678,124 678,136" fill="#888"/>
|
||||
</g>
|
||||
|
||||
<g font-family="Arial, sans-serif" font-size="14" fill="#222">
|
||||
<text x="150" y="334">heading 0</text>
|
||||
<text x="648" y="150">goal (x,y,theta)</text>
|
||||
<text x="470" y="210">Hermite trajectory</text>
|
||||
</g>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 1.4 KiB |
|
|
@ -1,6 +1,4 @@
|
|||
#include <mkt_algorithm/diff/pure_pursuit.h>
|
||||
#define LIMIT_VEL_THETA 0.33
|
||||
|
||||
|
||||
void mkt_algorithm::diff::PurePursuit::computePurePursuit(
|
||||
const score_algorithm::TrajectoryGenerator::Ptr &traj,
|
||||
|
|
@ -13,6 +11,7 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit(
|
|||
const double &lookahead_dist_max,
|
||||
const double &lookahead_dist,
|
||||
const double &lookahead_time,
|
||||
const double &dt,
|
||||
robot_nav_2d_msgs::Twist2D &drive_cmd)
|
||||
{
|
||||
if (!traj)
|
||||
|
|
@ -59,40 +58,126 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit(
|
|||
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 limit_acc_theta = fabs(v_theta) > 0.15 ? 1.0 : 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);
|
||||
|
||||
}
|
||||
|
||||
void mkt_algorithm::diff::PurePursuit::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, const double &sign_x)
|
||||
{
|
||||
double curvature_vel = linear_vel;
|
||||
double cost_vel = linear_vel;
|
||||
double approach_vel = linear_vel;
|
||||
|
||||
|
||||
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_);
|
||||
}
|
||||
}
|
||||
|
||||
if (use_cost_regulated_linear_velocity_scaling_ &&
|
||||
pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
|
||||
pose_cost != static_cast<double>(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);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
double mkt_algorithm::diff::PurePursuit::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
|
||||
}
|
||||
|
||||
|
||||
double mkt_algorithm::diff::PurePursuit::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
|
||||
}
|
||||
|
|
@ -105,7 +105,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
|
|||
catch (const std::exception &ex)
|
||||
{
|
||||
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
|
||||
exit(1);
|
||||
// exit(1);
|
||||
}
|
||||
|
||||
std::string algorithm_rotate_name;
|
||||
|
|
@ -120,7 +120,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
|
|||
if (!rotate_algorithm_)
|
||||
{
|
||||
robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
|
||||
exit(1);
|
||||
// exit(1);
|
||||
}
|
||||
rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
|
||||
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user