update
This commit is contained in:
parent
3f93370462
commit
575e190988
|
|
@ -81,7 +81,7 @@ endif()
|
|||
# Libraries
|
||||
# ========================================================
|
||||
add_library(${PROJECT_NAME}_diff SHARED
|
||||
src/diff/diff_predictive_trajectory_copy.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
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -12,8 +10,6 @@
|
|||
#include <robot_nav_core2/costmap.h>
|
||||
#include <nav_grid/coordinate_conversion.h>
|
||||
#include <angles/angles.h>
|
||||
#include <tf3/exceptions.h>
|
||||
#include <tf3/utils.h>
|
||||
#include <robot_nav_msgs/Path.h>
|
||||
#include <kalman/kalman.h>
|
||||
#include <vector>
|
||||
|
|
@ -44,7 +40,7 @@ namespace mkt_algorithm
|
|||
* @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;
|
||||
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
|
||||
|
|
@ -101,11 +97,6 @@ namespace mkt_algorithm
|
|||
*/
|
||||
virtual void getParams();
|
||||
|
||||
/**
|
||||
* @brief Initialize Kalman filter
|
||||
*/
|
||||
virtual void initKalmanFilter();
|
||||
|
||||
/**
|
||||
* @brief Dynamically adjust look ahead distance based on the speed
|
||||
* @param velocity
|
||||
|
|
@ -210,6 +201,8 @@ namespace mkt_algorithm
|
|||
const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
|
||||
const double &pose_cost, double &linear_vel, 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
|
||||
|
|
@ -229,6 +222,11 @@ namespace mkt_algorithm
|
|||
bool detectWobbleByCarrotAngle(std::vector<double>& angle_history, double current_angle,
|
||||
double amplitude_threshold = 0.3, size_t window_size = 20);
|
||||
|
||||
void publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose);
|
||||
|
||||
std::vector<double> angle_history_;
|
||||
size_t window_size_;
|
||||
|
||||
bool initialized_;
|
||||
bool nav_stop_;
|
||||
robot::NodeHandle nh_, nh_priv_;
|
||||
|
|
@ -278,6 +276,9 @@ namespace mkt_algorithm
|
|||
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_;
|
||||
|
|
|
|||
|
|
@ -1,27 +1,23 @@
|
|||
#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
|
||||
#include <mkt_algorithm/diff/pure_pursuit.h>
|
||||
#include <boost/dll/import.hpp>
|
||||
#include <robot/robot.h>
|
||||
|
||||
#include <boost/dll/alias.hpp>
|
||||
#define LIMIT_VEL_THETA 0.33
|
||||
|
||||
mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}
|
||||
|
||||
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);
|
||||
nh_ = robot::NodeHandle("~/");
|
||||
nh_priv_ = robot::NodeHandle("~/" + name);
|
||||
name_ = name;
|
||||
tf_ = tf;
|
||||
traj_ = traj;
|
||||
costmap_robot_ = costmap_robot;
|
||||
this->getParams();
|
||||
nh_.param("publish_topic", enable_publish_, false);
|
||||
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
|
||||
|
||||
footprint_spec_ = costmap_robot_->getRobotFootprint();
|
||||
std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
|
||||
if (footprint.size() > 1)
|
||||
{
|
||||
|
|
@ -44,64 +40,55 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
|
|||
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();
|
||||
|
||||
// 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<KalmanFilter>(dt, A, C, Q, R, P);
|
||||
Eigen::VectorXd x0(n_);
|
||||
x0 << 0, 0, 0, 0, 0, 0;
|
||||
kf_->init(robot::Time::now().toSec(), x0);
|
||||
|
||||
|
||||
|
||||
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<KalmanFilter>(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<std::string>("robot_base_frame", std::string("base_link"));
|
||||
robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
|
||||
nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
|
||||
nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
|
||||
nh_priv_.param<int>("index_samples", index_samples_, 0);
|
||||
|
|
@ -135,12 +122,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
|
|||
nh_priv_.param<double>("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.");
|
||||
robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
|
||||
use_cost_regulated_linear_velocity_scaling_ = false;
|
||||
}
|
||||
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_)
|
||||
{
|
||||
|
|
@ -298,6 +285,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
|
|||
return false;
|
||||
}
|
||||
|
||||
|
||||
x_direction = x_direction_;
|
||||
y_direction = y_direction_ = 0;
|
||||
theta_direction = theta_direction_;
|
||||
|
|
@ -346,6 +334,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
|
|||
|
||||
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;
|
||||
|
|
@ -375,7 +366,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;
|
||||
|
|
@ -390,15 +381,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
|
|||
}
|
||||
drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
|
||||
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__);
|
||||
|
|
@ -414,10 +397,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
|
|||
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);
|
||||
|
|
@ -440,11 +423,75 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
|
|||
}
|
||||
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);
|
||||
|
||||
// std::shared_ptr<PurePursuit> pure_pursuit = std::make_shared<PurePursuit>();
|
||||
// pure_pursuit->computePurePursuit(traj_, carrot_pose, velocity, min_approach_linear_velocity_, journey_plan, sign_x, lookahead_dist, lookahead_time_, drive_cmd);
|
||||
|
||||
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_)
|
||||
{
|
||||
|
|
@ -458,7 +505,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
|
|||
result.velocity = drive_cmd;
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
Eigen::VectorXd y(2);
|
||||
y << drive_cmd.x, drive_cmd.theta;
|
||||
|
||||
|
|
@ -709,7 +756,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
|
|||
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");
|
||||
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
|
||||
}
|
||||
|
||||
double dist_thresh_sq = dist_behind_robot * dist_behind_robot;
|
||||
|
|
@ -737,7 +784,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
|
|||
}
|
||||
catch (const tf3::TransformException &ex)
|
||||
{
|
||||
robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what());
|
||||
robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s\n", __FILE__, __LINE__, ex.what());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
|
@ -774,7 +821,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
|
|||
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");
|
||||
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
|
||||
}
|
||||
|
||||
// we'll discard points on the plan that are outside the local costmap
|
||||
|
|
@ -853,23 +900,23 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
|
|||
}
|
||||
catch (tf3::LookupException &ex)
|
||||
{
|
||||
robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what());
|
||||
robot::log_error("[%s:%d]\n No Transform available Error: %s\n", __FILE__, __LINE__, ex.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf3::ConnectivityException &ex)
|
||||
{
|
||||
robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what());
|
||||
robot::log_error("[%s:%d]\n Connectivity Error: %s\n", __FILE__, __LINE__, ex.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf3::ExtrapolationException &ex)
|
||||
{
|
||||
robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what());
|
||||
robot::log_error("[%s:%d]\n Extrapolation Error: %s\n", __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());
|
||||
robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
|
||||
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits(
|
||||
|
|
@ -912,7 +959,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_na
|
|||
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;
|
||||
|
|
@ -1016,6 +1062,49 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
|
|||
// ROS_INFO_STREAM_THROTTLE(0.1, ss.str());
|
||||
}
|
||||
|
||||
std::vector<robot_geometry_msgs::Point>
|
||||
mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
|
||||
{
|
||||
|
||||
// Dịch sang tọa độ tuyệt đối
|
||||
std::vector<robot_geometry_msgs::Point> 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<robot_geometry_msgs::Point> 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<int>(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<double>(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;
|
||||
|
|
@ -1093,11 +1182,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
|
|||
return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0;
|
||||
}
|
||||
|
||||
|
||||
score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
|
||||
{
|
||||
return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();
|
||||
}
|
||||
|
||||
// Register this planner as a GlobalPlanner plugin
|
||||
BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory)
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -120,9 +120,6 @@ namespace mkt_plugins
|
|||
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;
|
||||
|
|
|
|||
|
|
@ -14,7 +14,6 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter
|
|||
|
||||
void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt)
|
||||
{
|
||||
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<OneDVelocityIterator>("x_it_", current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
|
||||
kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_);
|
||||
|
||||
|
|
|
|||
|
|
@ -214,7 +214,10 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa
|
|||
{
|
||||
return;
|
||||
}
|
||||
// robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(local_plan_.poses.front());
|
||||
// pnkx_local_planner::transformGlobalPlan(tf_, local_plan_, local_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, path);
|
||||
path = local_plan_;
|
||||
|
||||
}
|
||||
|
||||
void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
|
||||
|
|
@ -234,12 +237,9 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
|
|||
}
|
||||
|
||||
// Update time stamp of goal pose
|
||||
// goal_pose_.header.stamp = pose.header.stamp;
|
||||
// robot::log_info("pose: %f %f %f", pose.pose.x, pose.pose.y, pose.pose.theta);
|
||||
robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(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_global_plan_))
|
||||
{
|
||||
|
|
@ -309,14 +309,14 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
|
|||
{
|
||||
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());
|
||||
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), 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());
|
||||
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
|
||||
local_plan_ = robot_nav_2d_utils::pathToPath(path);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user