first commit
This commit is contained in:
commit
377ebe3d6f
110
CMakeLists.txt
Executable file
110
CMakeLists.txt
Executable file
|
|
@ -0,0 +1,110 @@
|
|||
# --- CMake version và project name ---
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(custom_planner)
|
||||
|
||||
# --- C++ standard và position independent code ---
|
||||
set(CMAKE_CXX_STANDARD 17) # Sử dụng C++17
|
||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Thư viện có thể build thành shared lib
|
||||
|
||||
# --- RPATH settings: ưu tiên thư viện build tại chỗ ---
|
||||
# Dùng để runtime linker tìm thư viện đã build trước khi install
|
||||
set(CMAKE_SKIP_BUILD_RPATH FALSE)
|
||||
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
|
||||
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/custom_planner")
|
||||
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/custom_planner")
|
||||
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
|
||||
|
||||
# --- Dependencies ---
|
||||
# Tìm các thư viện cần thiết
|
||||
# find_package(tf3 REQUIRED) # Nếu dùng tf3
|
||||
find_package(Eigen3 REQUIRED) # Thư viện Eigen cho toán học
|
||||
find_package(Boost REQUIRED COMPONENTS system thread filesystem) # Boost: system, thread, filesystem
|
||||
|
||||
# --- Include directories ---
|
||||
# Thêm các folder chứa header files
|
||||
include_directories(
|
||||
include
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# --- Eigen và PCL definitions ---
|
||||
add_definitions(${EIGEN3_DEFINITIONS})
|
||||
|
||||
# --- Core library: custom_planner ---
|
||||
# Tạo thư viện chính
|
||||
add_library(custom_planner
|
||||
src/custom_planner.cpp
|
||||
src/pathway.cc
|
||||
src/pose.cc
|
||||
src/Curve_common.cpp
|
||||
src/merge_path_calc.cpp
|
||||
)
|
||||
|
||||
# --- Link các thư viện phụ thuộc ---
|
||||
target_link_libraries(custom_planner
|
||||
${Boost_LIBRARIES} # Boost
|
||||
visualization_msgs
|
||||
nav_msgs
|
||||
tf3
|
||||
tf3_geometry_msgs
|
||||
robot_time
|
||||
data_convert
|
||||
costmap_2d
|
||||
nav_core
|
||||
robot_protocol_msgs
|
||||
)
|
||||
|
||||
# --- Include directories cho target ---
|
||||
target_include_directories(custom_planner
|
||||
PUBLIC
|
||||
${Boost_INCLUDE_DIRS} # Boost headers
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> # Khi build từ source
|
||||
$<INSTALL_INTERFACE:include/${PROJECT_NAME}> # Khi install
|
||||
)
|
||||
|
||||
# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
|
||||
install(TARGETS custom_planner
|
||||
EXPORT custom_planner-targets
|
||||
ARCHIVE DESTINATION lib # Thư viện tĩnh .a
|
||||
LIBRARY DESTINATION lib # Thư viện động .so
|
||||
RUNTIME DESTINATION bin # File thực thi (nếu có)
|
||||
INCLUDES DESTINATION include # Cài đặt include
|
||||
)
|
||||
|
||||
# --- Xuất export set costmap_2dTargets thành file CMake module ---
|
||||
# --- Tạo file lib/cmake/custom_planner/costmap_2dTargets.cmake ---
|
||||
# --- File này chứa cấu hình giúp project khác có thể dùng ---
|
||||
# --- Find_package(custom_planner REQUIRED) ---
|
||||
# --- Target_link_libraries(my_app PRIVATE custom_planner::custom_planner) ---
|
||||
install(EXPORT custom_planner-targets
|
||||
FILE custom_planner-targets.cmake
|
||||
NAMESPACE custom_planner::
|
||||
DESTINATION lib/cmake/custom_planner
|
||||
)
|
||||
|
||||
# --- Cài đặt headers ---
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION include/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# # --- Plugin libraries ---
|
||||
# # Tạo các plugin shared library
|
||||
# add_library(plugins
|
||||
# SHARED
|
||||
# plugins/static_layer.cpp
|
||||
# plugins/obstacle_layer.cpp
|
||||
# plugins/inflation_layer.cpp
|
||||
# plugins/voxel_layer.cpp
|
||||
# plugins/critical_layer.cpp
|
||||
# plugins/directional_layer.cpp
|
||||
# plugins/preferred_layer.cpp
|
||||
# plugins/unpreferred_layer.cpp
|
||||
# )
|
||||
|
||||
# target_link_libraries(plugins
|
||||
# custom_planner
|
||||
# ${Boost_LIBRARIES}
|
||||
# yaml-cpp
|
||||
# robot_time
|
||||
# )
|
||||
17
config/custom_global_params.yaml
Executable file
17
config/custom_global_params.yaml
Executable file
|
|
@ -0,0 +1,17 @@
|
|||
base_global_planner: CustomPlanner
|
||||
CustomPlanner:
|
||||
environment_type: XYThetaLattice
|
||||
planner_type: ARAPlanner
|
||||
allocated_time: 10.0
|
||||
initial_epsilon: 1.0
|
||||
force_scratch_limit: 10000
|
||||
forward_search: false
|
||||
nominalvel_mpersecs: 0.8
|
||||
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
|
||||
allow_unknown: true
|
||||
directory_to_save_paths: "/init/paths"
|
||||
pathway_filename: "pathway.txt"
|
||||
current_pose_topic_name: "/amcl_pose"
|
||||
map_frame_id: "map"
|
||||
base_frame_id: "base_link"
|
||||
|
||||
84
include/custom_planner/Curve_common.h
Executable file
84
include/custom_planner/Curve_common.h
Executable file
|
|
@ -0,0 +1,84 @@
|
|||
#ifndef CUSTOM_PLANNER_CURVE_COMMON_H_
|
||||
#define CUSTOM_PLANNER_CURVE_COMMON_H_
|
||||
|
||||
//#include "agv_path_smoothing/conversion.h"
|
||||
#include <Eigen/Eigen>
|
||||
//#include <vector>
|
||||
// #include <deque>
|
||||
// #include <iostream>
|
||||
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include "custom_planner/color.h"
|
||||
|
||||
struct Spline_Inf
|
||||
{
|
||||
int order;
|
||||
std::vector<double> knot_vector;
|
||||
std::vector<double> weight;
|
||||
//std::vector<double> N;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point;
|
||||
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > N;
|
||||
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > dN;
|
||||
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > ddN;
|
||||
std::vector<double> N_double_vec;
|
||||
std::vector<double> R_double_vec;
|
||||
std::vector<double> dR_double_vec;
|
||||
std::vector<double> ddR_double_vec;
|
||||
// std::vector<double> ddN_double_vec;
|
||||
//Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, Eigen::Dynamic> N; //bsaic function
|
||||
Eigen::MatrixXd curvefit_N;
|
||||
|
||||
};
|
||||
|
||||
struct EigenTrajectoryPoint
|
||||
{
|
||||
typedef std::vector<EigenTrajectoryPoint, Eigen::aligned_allocator<EigenTrajectoryPoint> > Vector;
|
||||
double u_data;
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
Eigen::Vector3d position;
|
||||
//Eigen::VectorXd u_data;
|
||||
};
|
||||
|
||||
class Curve_common
|
||||
{
|
||||
public:
|
||||
Curve_common();
|
||||
nav_msgs::Path Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
|
||||
nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
|
||||
nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
|
||||
nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
|
||||
nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
|
||||
nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id);
|
||||
|
||||
void CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u_data, int differential_times);
|
||||
geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS);
|
||||
geometry_msgs::Point CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS);
|
||||
double CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
|
||||
double CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
|
||||
Eigen::Vector3d CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
|
||||
double CalculateCurvatureRadius(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
|
||||
double CalculateCurveLength(Spline_Inf spline_inf, double start_u, double end_u, int sub_intervals, bool UsingNURBS);
|
||||
|
||||
void ReadSplineInf(Spline_Inf *bspline_inf, int order, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point, std::vector<double> knot_vector);
|
||||
void ReadSplineInf(Spline_Inf *bspline_inf, std::vector<double> weight_vector, bool use_limit_derivative_fitting);
|
||||
void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector *input_point, std::vector<double> file_discreate_point);
|
||||
void ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > *input_point, std::vector<double> file_discreate_point);
|
||||
void ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point);
|
||||
void ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
|
||||
|
||||
//TODO: move relate visualize function to new vislization.h
|
||||
int print();
|
||||
visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
|
||||
visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
|
||||
visualization_msgs::Marker ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
|
||||
visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
|
||||
private:
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //CUSTOM_PLANNER_CURVE_COMMON_H_
|
||||
34
include/custom_planner/color.h
Executable file
34
include/custom_planner/color.h
Executable file
|
|
@ -0,0 +1,34 @@
|
|||
#ifndef CUSTOM_PLANNER_COLOR_H_
|
||||
#define CUSTOM_PLANNER_COLOR_H_
|
||||
|
||||
#include <std_msgs/ColorRGBA.h>
|
||||
|
||||
namespace agv_visualization
|
||||
{
|
||||
class Color : public std_msgs::ColorRGBA
|
||||
{
|
||||
public:
|
||||
Color() : std_msgs::ColorRGBA() {}
|
||||
Color(double red, double green, double blue) : Color(red, green, blue, 1.0) {}
|
||||
Color(double red, double green, double blue, double alpha) : Color() {
|
||||
r = red;
|
||||
g = green;
|
||||
b = blue;
|
||||
a = alpha;
|
||||
}
|
||||
|
||||
static const Color White() { return Color(1.0, 1.0, 1.0); }
|
||||
static const Color Black() { return Color(0.0, 0.0, 0.0); }
|
||||
static const Color Gray() { return Color(0.5, 0.5, 0.5); }
|
||||
static const Color Red() { return Color(1.0, 0.0, 0.0); }
|
||||
static const Color Green() { return Color(0.0, 1.0, 0.0); }
|
||||
static const Color Blue() { return Color(0.0, 0.0, 1.0); }
|
||||
static const Color Yellow() { return Color(1.0, 1.0, 0.0); }
|
||||
static const Color Orange() { return Color(1.0, 0.5, 0.0); }
|
||||
static const Color Purple() { return Color(0.5, 0.0, 1.0); }
|
||||
static const Color Chartreuse() { return Color(0.5, 1.0, 0.0); }
|
||||
static const Color Teal() { return Color(0.0, 1.0, 1.0); }
|
||||
static const Color Pink() { return Color(1.0, 0.0, 0.5); }
|
||||
};
|
||||
}
|
||||
#endif //CUSTOM_PLANNER_COLOR_H_
|
||||
43
include/custom_planner/conversion.h
Executable file
43
include/custom_planner/conversion.h
Executable file
|
|
@ -0,0 +1,43 @@
|
|||
#ifndef CUSTOM_PLANNER_CONVERSION_H_
|
||||
#define CUSTOM_PLANNER_CONVERSION_H_
|
||||
#include "custom_planner/Curve_common.h"
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
|
||||
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) {
|
||||
return Eigen::Vector3d(msg.x, msg.y, msg.z);
|
||||
}
|
||||
|
||||
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::PoseStamped> &plan)
|
||||
{
|
||||
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
|
||||
EigenTrajectoryPoint eigen_trajectory_point;
|
||||
|
||||
for(int i = 0; i < static_cast<int>(plan.size()); i++)
|
||||
{
|
||||
eigen_trajectory_point.position(0) = plan.at(i).pose.position.x;
|
||||
eigen_trajectory_point.position(1) = plan.at(i).pose.position.y;
|
||||
eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
|
||||
}
|
||||
|
||||
return eigen_trajectory_point_vec;
|
||||
}
|
||||
|
||||
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::Point> &discreate_point_vec)
|
||||
{
|
||||
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
|
||||
EigenTrajectoryPoint eigen_trajectory_point;
|
||||
|
||||
for(int i = 0; i < static_cast<int>(discreate_point_vec.size()); i++)
|
||||
{
|
||||
eigen_trajectory_point.position(0) = discreate_point_vec.at(i).x;
|
||||
eigen_trajectory_point.position(1) = discreate_point_vec.at(i).y;
|
||||
eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
|
||||
}
|
||||
|
||||
return eigen_trajectory_point_vec;
|
||||
}
|
||||
|
||||
#endif //CUSTOM_PLANNER_CONVERSION_H_
|
||||
230
include/custom_planner/custom_planner.h
Executable file
230
include/custom_planner/custom_planner.h
Executable file
|
|
@ -0,0 +1,230 @@
|
|||
#ifndef CUSTOM_PLANNER_H
|
||||
#define CUSTOM_PLANNER_H
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <map>
|
||||
|
||||
using namespace std;
|
||||
|
||||
// ROS
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
|
||||
// Costmap used for the map representation
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
|
||||
// global representation
|
||||
#include <nav_core/base_global_planner.h>
|
||||
|
||||
// tf
|
||||
#include <tf3/convert.h>
|
||||
#include <tf3/buffer_core.h>
|
||||
|
||||
// internal lib
|
||||
#include <custom_planner/pose.h>
|
||||
#include <custom_planner/pathway.h>
|
||||
#include "custom_planner/Curve_common.h"
|
||||
#include "custom_planner/conversion.h"
|
||||
#include "custom_planner/merge_path_calc.h"
|
||||
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
|
||||
#include <thread>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include <robot_protocol_msgs/Order.h>
|
||||
// #include "vda5050_msgs/Order.h"
|
||||
// #include "vda5050_msgs/Trajectory.h"
|
||||
// #include "vda5050_msgs/Edge.h"
|
||||
// #include "vda5050_msgs/Node.h"
|
||||
// #include "vda5050_msgs/ControlPoint.h"
|
||||
// #include "vda5050_msgs/NodePosition.h"
|
||||
|
||||
const double EPSILON = 1e-6;
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace custom_planner{
|
||||
|
||||
struct OrderNode{
|
||||
string nodeId;
|
||||
uint32_t sequenceId;
|
||||
double position_x;
|
||||
double position_y;
|
||||
double theta;
|
||||
};
|
||||
|
||||
class CustomPlanner : public nav_core::BaseGlobalPlanner{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Default constructor for the NavFnROS object
|
||||
*/
|
||||
CustomPlanner();
|
||||
|
||||
|
||||
/**
|
||||
* @brief Constructor for the CustomPlanner object
|
||||
* @param name The name of this planner
|
||||
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use
|
||||
*/
|
||||
CustomPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initialization function for the CustomPlanner object
|
||||
* @param name The name of this planner
|
||||
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use
|
||||
*/
|
||||
virtual bool initialize(std::string name,
|
||||
costmap_2d::Costmap2DROBOT* costmap_robot);
|
||||
|
||||
virtual bool makePlan(const robot_protocol_msgs::Order& msg,
|
||||
const geometry_msgs::PoseStamped& start,
|
||||
const geometry_msgs::PoseStamped& goal,
|
||||
std::vector<geometry_msgs::PoseStamped>& plan);
|
||||
/**
|
||||
* @brief Given a goal pose in the world, compute a plan
|
||||
* @param start The start pose
|
||||
* @param goal The goal pose
|
||||
* @param plan The plan... filled by the planner
|
||||
* @return True if a valid plan was found, false otherwise
|
||||
*/
|
||||
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
|
||||
const geometry_msgs::PoseStamped& goal,
|
||||
std::vector<geometry_msgs::PoseStamped>& plan)
|
||||
{
|
||||
printf("[%s:%d] This function is not available!",__FILE__,__LINE__);
|
||||
return false;
|
||||
}
|
||||
|
||||
virtual ~CustomPlanner(){};
|
||||
|
||||
private:
|
||||
void publishStats(int solution_cost, int solution_size,
|
||||
const geometry_msgs::PoseStamped& start,
|
||||
const geometry_msgs::PoseStamped& goal);
|
||||
|
||||
static void transformFootprintToEdges(const geometry_msgs::Pose& robot_pose,
|
||||
const std::vector<geometry_msgs::Point>& footprint,
|
||||
std::vector<geometry_msgs::Point>& out_footprint);
|
||||
|
||||
Pose pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length = 1.0);
|
||||
|
||||
bool countRobotDirectionChangeAngle(vector<Pose>& savePosesOnEdge, int& total_edge);
|
||||
|
||||
inline double calculateAngle(double xA, double yA, double xB, double yB)
|
||||
{
|
||||
double deltaX = xB - xA;
|
||||
double deltaY = yB - yA;
|
||||
double angleRad = atan2(deltaY, deltaX);
|
||||
// double angleDeg = angleRad * 180.0 / M_PI;
|
||||
return angleRad;
|
||||
}
|
||||
|
||||
inline bool isOppositeSign(double angleA, double angleB)
|
||||
{
|
||||
return (angleA > 0 && angleB < 0) || (angleA < 0 && angleB > 0);
|
||||
}
|
||||
|
||||
bool checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose,
|
||||
const geometry_msgs::PoseStamped &goal);
|
||||
|
||||
bool loadPathwayData(const string& filename);
|
||||
|
||||
bool findNearestPoseOfPath(vector<Pose>& posesOnPathWay, Pose& PoseToCheck, Pose& PoseResult);
|
||||
|
||||
// void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg);
|
||||
|
||||
bool makePlanWithOrder(robot_protocol_msgs::Order msg,
|
||||
const geometry_msgs::PoseStamped& start,
|
||||
const geometry_msgs::PoseStamped& goal);
|
||||
|
||||
bool doIntersect(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
|
||||
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2);
|
||||
|
||||
std::optional<geometry_msgs::Point> computeIntersectionPoint(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
|
||||
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2);
|
||||
|
||||
bool calcAllYaw(const geometry_msgs::PoseStamped& start,
|
||||
const geometry_msgs::PoseStamped& goal,
|
||||
vector<Pose>& saveposesOnEdge,
|
||||
int& total_edge);
|
||||
|
||||
bool isThetaValid(double theta);
|
||||
|
||||
bool curveIsValid(int degree, const std::vector<double> &knot_vector,
|
||||
vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);
|
||||
|
||||
void setYawAllPosesOnEdge(vector<Pose>& posesOnEdge, bool reverse);
|
||||
|
||||
double computeDeltaAngle(Pose& Pose1, Pose& Pose2);
|
||||
|
||||
vector<Pose> divideSegment(Pose& A, Pose& B, double d);
|
||||
vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped& A, geometry_msgs::PoseStamped& B, double d);
|
||||
|
||||
// Vector lưu thông tin các cạnh
|
||||
std::vector<InfEdge> edges_info_;
|
||||
std::vector<RobotDirectionChangeAngle> RobotDirectionChangeAngle_info_;
|
||||
|
||||
MergePathCalc merge_path_calc_;
|
||||
bool skipEdge_flag_ = false;
|
||||
bool reverse_ = false;
|
||||
|
||||
Spline_Inf* input_spline_inf;
|
||||
Curve_common* CurveDesign;
|
||||
Pathway* pathway;
|
||||
Pose* startPose;
|
||||
vector<Pose> posesOnPathWay;
|
||||
Pose start_on_path;
|
||||
std::map<string, OrderNode> orderNodes;
|
||||
|
||||
// vda5050_msgs::Order order_msg_;
|
||||
uint16_t start_on_path_index;
|
||||
bool initialized_;
|
||||
|
||||
bool rotating_robot_plag_ = false; /**< whether the robot is rotating or not, used to determine the direction of the path */
|
||||
|
||||
std::string planner_type_; /**< sbpl method to use for planning. choices are ARAPlanner and ADPlanner */
|
||||
|
||||
double allocated_time_; /**< amount of time allowed for search */
|
||||
double initial_epsilon_; /**< initial epsilon for beginning the anytime search */
|
||||
|
||||
std::string environment_type_; /** what type of environment in which to plan. choices are 2D and XYThetaLattice. */
|
||||
std::string cost_map_topic_; /** what topic is being used for the costmap topic */
|
||||
|
||||
bool forward_search_; /** whether to use forward or backward search */
|
||||
std::string primitive_filename_; /** where to find the motion primitives for the current robot */
|
||||
int force_scratch_limit_; /** the number of cells that have to be changed in the costmap to force the planner to plan from scratch even if its an incremental planner */
|
||||
|
||||
unsigned char lethal_obstacle_;
|
||||
unsigned char inscribed_inflated_obstacle_;
|
||||
unsigned char circumscribed_cost_;
|
||||
unsigned char sbpl_cost_multiplier_;
|
||||
|
||||
bool publish_footprint_path_;
|
||||
int visualizer_skip_poses_;
|
||||
|
||||
bool allow_unknown_;
|
||||
|
||||
std::string name_;
|
||||
costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
|
||||
std::vector<geometry_msgs::Point> footprint_;
|
||||
unsigned int current_env_width_;
|
||||
unsigned int current_env_height_;
|
||||
|
||||
// ros::Subscriber order_msg_sub_;
|
||||
// ros::Publisher plan_pub_;
|
||||
// ros::Publisher stats_publisher_;
|
||||
|
||||
// vector<ros::ServiceServer> service_servers_;
|
||||
|
||||
// ros::Publisher sbpl_plan_footprint_pub_;
|
||||
boost::mutex mutex_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
214
include/custom_planner/merge_path_calc.h
Executable file
214
include/custom_planner/merge_path_calc.h
Executable file
|
|
@ -0,0 +1,214 @@
|
|||
#ifndef MERGER_PATH_CALC_H_
|
||||
#define MERGER_PATH_CALC_H_
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
|
||||
#include <tf3/convert.h>
|
||||
|
||||
#include "custom_planner/pose.h"
|
||||
#include "custom_planner/Curve_common.h"
|
||||
#include <data_convert/data_convert.h>
|
||||
|
||||
namespace custom_planner
|
||||
{
|
||||
//Lưu thông tin các cạnh
|
||||
struct InfEdge{
|
||||
int start_edge_idx;
|
||||
int end_edge_idx;
|
||||
bool isCurve;
|
||||
double lenght_edge;
|
||||
};
|
||||
|
||||
//Lưu trữ gosc chuyển hướng
|
||||
struct RobotDirectionChangeAngle{
|
||||
int start_edge_idx_first;
|
||||
int end_edge_idx_first;
|
||||
int start_edge_idx_second;
|
||||
int end_edge_idx_second;
|
||||
};
|
||||
|
||||
enum point_type
|
||||
{
|
||||
START = 0,
|
||||
GOAL = 1,
|
||||
UNKNOWN = 3
|
||||
};
|
||||
class MergePathCalc
|
||||
{
|
||||
public:
|
||||
MergePathCalc();
|
||||
~MergePathCalc();
|
||||
|
||||
/**
|
||||
* \brief Tính toán bước nhảy thích hợp dựa trên độ dài đoạn và khoảng cách điểm mong muốn.
|
||||
* \param segment_length Độ dài của đoạn đường.
|
||||
* \param desired_point_spacing Khoảng cách giữa các điểm mong muốn (mặc định là 0.02).
|
||||
* \return Trả về bước nhảy thích hợp để lấy mẫu điểm trên đoạn đường.
|
||||
* \details
|
||||
* Hàm này sẽ tính toán bước nhảy dựa trên độ dài đoạn đường và khoảng cách giữa các điểm mong muốn.
|
||||
*/
|
||||
inline double calculateAdaptiveStep(double segment_length,
|
||||
double desired_point_spacing = 0.02)
|
||||
{
|
||||
return desired_point_spacing / segment_length;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Tìm điểm gần nhất trên đường đi.
|
||||
* \param pose Vị trí hiện tại của robot.
|
||||
* \param posesOnPathWay Các điểm trên đường đi.
|
||||
* \return Trả về chỉ mục của điểm gần nhất trong posesOnPathWay.
|
||||
* \details
|
||||
* Hàm này sẽ tìm điểm gần nhất trên đường đi so với vị trí hiện tại của robot.
|
||||
* Nếu khoảng cách đến điểm gần nhất nhỏ hơn biding_distance, nó sẽ
|
||||
* tiếp tục tìm điểm xa hơn để đảm bảo ràng buộc khoảng cách.
|
||||
* Nếu tìm thấy, nó sẽ trả về chỉ mục của điểm gần nhất trong posesOnPathWay.
|
||||
* Nếu không tìm thấy điểm nào thỏa mãn, nó sẽ trả về -1.
|
||||
*/
|
||||
int findNearestPointOnPath(Pose& pose,
|
||||
std::vector<Pose>& posesOnPathWay,
|
||||
point_type type);
|
||||
|
||||
/**
|
||||
* \brief Tính độ dài của đường NURBS.
|
||||
* \param curve_design Con trỏ đến đối tượng CurveCommon chứa các hàm tính toán đường cong.
|
||||
* \param spline_inf Con trỏ đến đối tượng Spline_Inf chứa thông tin về đường cong NURBS.
|
||||
* \param degree Bậc của đường NURBS (1, 2 hoặc 3).
|
||||
* \param initial_step Bước nhảy ban đầu để lấy mẫu điểm trên đường cong.
|
||||
* \return Trả về độ dài của đường NURBS.
|
||||
* \details
|
||||
* Hàm này sẽ tính độ dài của đường NURBS bằng cách lấy mẫu các điểm trên đường cong
|
||||
* và tính khoảng cách giữa các điểm liên tiếp. Nó sẽ trả về tổng độ dài của đường cong.
|
||||
*/
|
||||
double calculateNURBSLength(Curve_common* curve_design,
|
||||
Spline_Inf* spline_inf,
|
||||
double initial_step = 0.01);
|
||||
|
||||
/**
|
||||
* \brief Tìm các điểm điều khiển NURBS.
|
||||
* \param start Vị trí bắt đầu.
|
||||
* \param goal Vị trí kết thúc.
|
||||
* \param posesOnPathWay Các điểm trên đường đi.
|
||||
* \param degree Bậc của đường cong NURBS (2 hoặc 3).
|
||||
* \return Trả về một vector chứa các điểm điều khiển NURBS.
|
||||
* \details
|
||||
* Hàm này tìm các điểm điều khiển NURBS dựa trên vị trí bắt đầu, kết thúc và các điểm trên đường đi.
|
||||
* Nó sử dụng bậc của đường cong NURBS để xác định số lượng điểm điều khiển cần thiết.
|
||||
*/
|
||||
bool findNURBSControlPoints(vector<geometry_msgs::PoseStamped>& control_points,
|
||||
const geometry_msgs::PoseStamped& start,
|
||||
std::vector<Pose>& posesOnPathWay,
|
||||
point_type type);
|
||||
|
||||
/**
|
||||
* \brief Tính toán đường đi NURBS.
|
||||
* \param degree Bậc của đường NURBS (2 hoặc 3).
|
||||
* \return Trả về một vector chứa các PoseStamped trên đường đi NURBS.
|
||||
* \details
|
||||
* Hàm này sẽ tính toán đường đi NURBS dựa trên các điểm điều khiển đã tìm được.
|
||||
* Nó sẽ sử dụng các hàm từ CurveCommon và Spline_Inf để tính toán các điểm trên đường cong.
|
||||
*/
|
||||
std::vector<geometry_msgs::PoseStamped> calculateNURBSPath(vector<geometry_msgs::PoseStamped>& control_points,
|
||||
bool reverse);
|
||||
|
||||
bool handleEdgeIntersection(vector<Pose>& savePosesOnEdge,
|
||||
vector<Pose>& posesOnEdge,
|
||||
size_t current_edge_idx);
|
||||
|
||||
/**
|
||||
* \brief Tính toán góc giữa hai điểm.
|
||||
* \param x1 Tọa độ x của điểm đầu tiên.
|
||||
* \param y1 Tọa độ y của điểm đầu tiên.
|
||||
* \param x2 Tọa độ x của điểm thứ hai.
|
||||
* \param y2 Tọa độ y của điểm thứ hai.
|
||||
* \return Trả về góc tính bằng radian từ điểm đầu đến điểm thứ hai.
|
||||
* \details
|
||||
* Hàm này sẽ tính toán góc giữa hai điểm (x1, y1) và (x2, y2) theo công thức atan2.
|
||||
*/
|
||||
inline double calculateAngle(double x1, double y1, double x2, double y2)
|
||||
{
|
||||
double angle = atan2(y2 - y1, x2 - x1);
|
||||
return angle;
|
||||
}
|
||||
|
||||
inline double normalizeAngle(double angle)
|
||||
{
|
||||
while (angle > M_PI)
|
||||
angle -= 2.0 * M_PI;
|
||||
while (angle <= -M_PI)
|
||||
angle += 2.0 * M_PI;
|
||||
return angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Tính góc có hướng giữa hai vector.
|
||||
* \param start_pose Vị trí bắt đầu.
|
||||
* \param posesOnPathWay Các điểm trên đường đi.
|
||||
* \param idx Chỉ mục của điểm cần tính trong posesOnPathWay.
|
||||
* \return Trả về góc có hướng giữa hai vector.
|
||||
* \details
|
||||
* Hàm này sẽ tính góc có hướng giữa vector từ start_pose đến điểm hiện tại
|
||||
* và vector từ điểm hiện tại đến điểm tiếp theo trong posesOnPathWay.
|
||||
*/
|
||||
double signedAngle(Pose& start_pose,
|
||||
std::vector<Pose>& posesOnPathWay,
|
||||
point_type type,
|
||||
int& idx);
|
||||
|
||||
double signedAngle(Pose& start_pose, Pose& mid_pose, Pose& end_pose);
|
||||
|
||||
// Chỉ mục của điểm mục tiêu trong posesOnPathWay
|
||||
int start_target_idx_ = 0;
|
||||
int goal_target_idx_ = 0;
|
||||
int store_start_nearest_idx_ = 0;
|
||||
int store_goal_nearest_idx_ = 0;
|
||||
int end_idx_;
|
||||
|
||||
int edge_front_start_idx_ = 0;
|
||||
int edge_front_end_idx_ = 0;
|
||||
int edge_back_start_idx_ = 0;
|
||||
int edge_back_end_idx_ = 0;
|
||||
|
||||
bool status_yaw_edge_end_ = true;
|
||||
|
||||
|
||||
private:
|
||||
// vector<geometry_msgs::PoseStamped> control_points;
|
||||
|
||||
bool normal_plag = false;
|
||||
|
||||
// CurveCommon* curve_design = nullptr;
|
||||
// Spline_Inf* spline_inf = nullptr;
|
||||
Spline_Inf* spline_inf = nullptr;
|
||||
Curve_common* curve_design = nullptr;
|
||||
|
||||
/**
|
||||
* \brief Cập nhật hướng của các Pose trong đường đi NURBS.
|
||||
* \param saved_poses Vector chứa các Pose đã lưu.
|
||||
* \param nurbs_path Vector chứa đường đi NURBS.
|
||||
* \details
|
||||
* Hàm này sẽ cập nhật hướng (yaw) của các Pose trong saved_poses dựa trên hướng của đường đi NURBS.
|
||||
* Nó sẽ tính toán góc yaw dựa trên vị trí của các Pose và cập nhật chúng.
|
||||
*/
|
||||
void updatePoseOrientation(std::vector<geometry_msgs::PoseStamped>& saved_poses,
|
||||
std::vector<geometry_msgs::PoseStamped>& nurbs_path,
|
||||
bool reverse);
|
||||
|
||||
/**
|
||||
* \brief Thiết lập hướng (yaw) của các Pose trên đường NuRBS.
|
||||
* \param Poses Vector chứa các PoseStamped.
|
||||
* \param reverse Nếu true, sẽ đảo ngược hướng của các Pose.
|
||||
* \details
|
||||
* Hàm này sẽ thiết lập hướng (yaw) cho tất cả các Pose trong vector Poses.
|
||||
* Nếu reverse là true, nó sẽ đảo ngược hướng của các Pose.
|
||||
* Nếu reverse là false, nó sẽ giữ nguyên hướng của các Pose.
|
||||
*/
|
||||
static void setYawAllPosesOnEdge(std::vector<geometry_msgs::PoseStamped>& Poses,
|
||||
bool reverse = false);
|
||||
|
||||
|
||||
};
|
||||
|
||||
} // namespace custom_planner
|
||||
#endif // MERGE_PATH_CALC_H_
|
||||
150
include/custom_planner/pathway.h
Executable file
150
include/custom_planner/pathway.h
Executable file
|
|
@ -0,0 +1,150 @@
|
|||
#ifndef CUSTOM_PLANNER_CUSTOM_PLANNER_PATHWAY_H
|
||||
#define CUSTOM_PLANNER_CUSTOM_PLANNER_PATHWAY_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include <iostream>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
#include <custom_planner/pose.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace custom_planner
|
||||
{
|
||||
|
||||
class Point {
|
||||
private:
|
||||
double x_, y_;
|
||||
|
||||
public:
|
||||
Point():
|
||||
x_(0.0), y_(0.0) {};
|
||||
|
||||
Point(double x, double y):
|
||||
x_(x), y_(y) {};
|
||||
|
||||
~Point() {};
|
||||
|
||||
inline void setX(double x) { x_ = x; }
|
||||
inline void setY(double y) { y_ = y; }
|
||||
inline void setPoint(double x, double y) { x_ = x, y_ = y; }
|
||||
inline void setPoint(Point p) { x_ = p.x_, y_ = p.y_; }
|
||||
|
||||
inline double getX(void) { return x_; }
|
||||
inline double getY(void) { return y_; }
|
||||
inline Point getPoint(void) { return Point(x_, y_); }
|
||||
|
||||
}; // class Point
|
||||
|
||||
class Edge {
|
||||
private:
|
||||
Pose p1_;
|
||||
Pose p2_;
|
||||
public:
|
||||
Edge(void):
|
||||
p1_(Pose(0.0, 0.0, 0.0)),
|
||||
p2_(Pose(0.0, 0.0, 0.0)) {}
|
||||
Edge(Pose p1, Pose p2):
|
||||
p1_(p1),
|
||||
p2_(p2) {}
|
||||
inline void setEdge(Pose p1, Pose p2){
|
||||
p1_ = p1; p2_ = p2;
|
||||
}
|
||||
~Edge() {}
|
||||
inline void setP1(Pose p1){p1_ = p1;}
|
||||
inline void setP2(Pose p2){p2_ = p2;}
|
||||
inline Pose getP1(void){return p1_;}
|
||||
inline Pose getP2(void){return p2_;}
|
||||
};
|
||||
|
||||
class Pathway {
|
||||
private:
|
||||
std::vector<Edge> path_;
|
||||
vector<Pose> posesOnPath_;
|
||||
Pose startPoseOfPath;
|
||||
// Given three collinear poses p, q, r, the function checks if
|
||||
// pose q lies on edge 'pr'
|
||||
bool onSegment(Pose p, Pose q, Pose r);
|
||||
|
||||
//To find orientation of ordered triplet (p, q, r)
|
||||
// The function returns following values
|
||||
// 0 --> p, q and r are collinear
|
||||
// 1 --> Clockwise
|
||||
// 2 --> Counterclockwise
|
||||
int orientation(Pose p, Pose q, Pose r);
|
||||
|
||||
bool doIntersect(Pose p1, Pose q1, Pose p2, Pose q2);
|
||||
|
||||
bool isIntersect(Edge L);
|
||||
|
||||
bool isPoseExisted(Pose p);
|
||||
|
||||
double calculateAngle(Pose p1, Pose p2, Pose p3);
|
||||
|
||||
bool findIntersection(Edge L1, Edge L2, Point* resultPoint);
|
||||
|
||||
public:
|
||||
bool isPathInitilized_;
|
||||
bool isPathClear_;
|
||||
Pathway():isPathInitilized_(false),isPathClear_(false)
|
||||
{
|
||||
|
||||
// startPointOfPath = Pose()
|
||||
|
||||
}
|
||||
~Pathway(){
|
||||
path_.clear();
|
||||
posesOnPath_.clear();
|
||||
}
|
||||
void RecordPath(Pose startPose, Pose currentPose, double delta_angle_th, double InitialSegmentLengthMin, double SegmentLengthMin);
|
||||
void ResetAndRecordPath(Pose startPose, Pose currentPose, double delta_angle_th, double InitialSegmentLengthMin, double SegmentLengthMin);
|
||||
inline void clear(){path_.clear();}
|
||||
inline bool isPathEmpty(){return path_.empty();}
|
||||
inline void addEdge(Pose p1, Pose p2) {path_.push_back(Edge(p1, p2));}
|
||||
inline void addEdge(Edge Edge) {path_.push_back(Edge);}
|
||||
inline void addEdge(Pose p2) {if(!path_.empty()&&!isPoseExisted(p2))
|
||||
path_.push_back(Edge(path_.back().getP2(),p2));}
|
||||
|
||||
bool isNewPoseOnEdge(Pose p, double delta_angle_th);
|
||||
|
||||
inline void setP2LastEdgeOnPath(Pose p2)
|
||||
{
|
||||
path_.back().setP2(p2);
|
||||
}
|
||||
|
||||
void setEdgeOnPath(uint32_t line_index, Pose p1, Pose p2);
|
||||
|
||||
inline vector<Pose> getPosesOnPath(){return posesOnPath_;}
|
||||
|
||||
void syncPosesAndPath();
|
||||
|
||||
void SavePathAsFile(const string& file_name);
|
||||
|
||||
void LoadPathFromFile(const string& file_name);
|
||||
|
||||
vector<Point> findIntersections(Edge L);
|
||||
|
||||
vector<vector<Point>> findIntersections(Edge L1, Edge L2);
|
||||
|
||||
void testprint()
|
||||
{
|
||||
if(!path_.empty()){
|
||||
int i =0;
|
||||
for(auto &edge : path_)
|
||||
{
|
||||
cout<<"edge "<<i<<": "<<edge.getP1().getX()
|
||||
<<","<<edge.getP1().getY()<<","<<edge.getP1().getYaw()<<" "
|
||||
<<edge.getP2().getX()<<","<<edge.getP2().getY()<<","<<edge.getP2().getYaw()<<endl;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace custom_planner
|
||||
|
||||
#endif // CUSTOM_PLANNER_CUSTOM_PLANNER_PATHWAY_H
|
||||
52
include/custom_planner/pose.h
Executable file
52
include/custom_planner/pose.h
Executable file
|
|
@ -0,0 +1,52 @@
|
|||
#ifndef CUSTOM_PLANNER_CUSTOM_PLANNER_POSE_H
|
||||
#define CUSTOM_PLANNER_CUSTOM_PLANNER_POSE_H
|
||||
#include <stdio.h>
|
||||
#include <iostream>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
#include <fstream>
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
namespace custom_planner{
|
||||
|
||||
class Pose {
|
||||
private:
|
||||
double x_, y_, yaw_;
|
||||
|
||||
void modifyYaw(void) {
|
||||
while (yaw_ < -M_PI)
|
||||
yaw_ += 2.0 * M_PI;
|
||||
while (yaw_ > M_PI)
|
||||
yaw_ -= 2.0 * M_PI;
|
||||
}
|
||||
|
||||
public:
|
||||
Pose():
|
||||
x_(0.0), y_(0.0), yaw_(0.0) {};
|
||||
|
||||
Pose(double x, double y, double yaw):
|
||||
x_(x), y_(y), yaw_(yaw) {};
|
||||
|
||||
~Pose() {};
|
||||
|
||||
inline void setX(double x) { x_ = x; }
|
||||
inline void setY(double y) { y_ = y; }
|
||||
inline void setYaw(double yaw) { yaw_ = yaw, modifyYaw(); }
|
||||
inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); }
|
||||
inline void setPose(Pose p) { x_ = p.x_, y_ = p.y_, yaw_ = p.yaw_, modifyYaw(); }
|
||||
|
||||
inline double getX(void) { return x_; }
|
||||
inline double getY(void) { return y_; }
|
||||
inline double getYaw(void) { return yaw_; }
|
||||
inline Pose getPose(void) { return Pose(x_, y_, yaw_); }
|
||||
bool SavePoseAsFile(const string& file_name);
|
||||
bool LoadPoseFromFile(const string& file_name);
|
||||
|
||||
}; // class Pose
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
1269
src/Curve_common.cpp
Executable file
1269
src/Curve_common.cpp
Executable file
File diff suppressed because it is too large
Load Diff
1195
src/custom_planner.cpp
Executable file
1195
src/custom_planner.cpp
Executable file
File diff suppressed because it is too large
Load Diff
673
src/merge_path_calc.cpp
Executable file
673
src/merge_path_calc.cpp
Executable file
|
|
@ -0,0 +1,673 @@
|
|||
#include"custom_planner/merge_path_calc.h"
|
||||
|
||||
namespace custom_planner {
|
||||
|
||||
MergePathCalc::MergePathCalc(){
|
||||
curve_design = new Curve_common();
|
||||
spline_inf = new Spline_Inf();
|
||||
}
|
||||
MergePathCalc::~MergePathCalc(){
|
||||
// Giải phóng bộ nhớ
|
||||
delete curve_design;
|
||||
delete spline_inf;
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<geometry_msgs::PoseStamped>& control_points, bool reverse)
|
||||
{
|
||||
std::vector<geometry_msgs::PoseStamped> nurbs_path;
|
||||
std::vector<geometry_msgs::PoseStamped> saved_poses; // Lưu các pose đã tính toán
|
||||
|
||||
if((int)nurbs_path.size() > 0)
|
||||
{
|
||||
nurbs_path.clear(); // Xóa đường cong cũ nếu có
|
||||
saved_poses.clear(); // Xóa các pose đã lưu
|
||||
}
|
||||
|
||||
// Chuyển đổi control points sang định dạng Eigen::Vector3d
|
||||
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> eigen_control_points;
|
||||
for(const auto& cp : control_points)
|
||||
{
|
||||
Eigen::Vector3d point(cp.pose.position.x, cp.pose.position.y, 0);
|
||||
eigen_control_points.push_back(point);
|
||||
}
|
||||
// Tạo knot vector tự động
|
||||
std::vector<double> knot_vector;
|
||||
int degree = 2;
|
||||
int n = control_points.size() - 1; // n + 1 control points
|
||||
int m = n + degree + 1; // m + 1 knots
|
||||
// Tạo uniform knot vector
|
||||
for(int i = 0; i <= m; i++)
|
||||
{
|
||||
if(i <= degree) knot_vector.push_back(0.0);
|
||||
else knot_vector.push_back(1.0);
|
||||
// else knot_vector.push_back((double)(i - degree)/(m - 2*degree));
|
||||
}
|
||||
|
||||
// Điều chỉnh weights để tăng ảnh hưởng của các control points
|
||||
std::vector<double> weights(control_points.size(), 1.0);
|
||||
|
||||
spline_inf->knot_vector.clear();
|
||||
spline_inf->control_point.clear();
|
||||
spline_inf->weight.clear();
|
||||
|
||||
// Cài đặt thông số cho spline
|
||||
curve_design->ReadSplineInf(spline_inf, degree + 1, eigen_control_points, knot_vector);
|
||||
curve_design->ReadSplineInf(spline_inf, weights, false);
|
||||
|
||||
// Tính độ dài đường cong để xác định số điểm cần lấy mẫu
|
||||
double NURBS_length = calculateNURBSLength(curve_design, spline_inf, 0.01);
|
||||
double desired_spacing = 0.05; // Khoảng cách mong muốn giữa các điểm
|
||||
double step = calculateAdaptiveStep(NURBS_length, desired_spacing);
|
||||
|
||||
for(double t = 0.0; t <= 1.0; t += step)
|
||||
{
|
||||
geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
|
||||
if(!std::isnan(point.x) && !std::isnan(point.y))
|
||||
{
|
||||
geometry_msgs::PoseStamped pose;
|
||||
pose.header.stamp = robot::Time::now();
|
||||
pose.pose.position = point;
|
||||
pose.pose.orientation.x = 0.0;
|
||||
pose.pose.orientation.y = 0.0;
|
||||
pose.pose.orientation.z = 0.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
|
||||
// nurbs_path.push_back(pose);
|
||||
saved_poses.push_back(pose);
|
||||
if(saved_poses.size() > 1)
|
||||
{
|
||||
updatePoseOrientation(saved_poses, nurbs_path, reverse);
|
||||
}
|
||||
}
|
||||
}
|
||||
if(!saved_poses.empty())
|
||||
{
|
||||
double end_yaw;
|
||||
if(!reverse)
|
||||
end_yaw = calculateAngle(saved_poses.back().pose.position.x,
|
||||
saved_poses.back().pose.position.y,
|
||||
control_points.back().pose.position.x,
|
||||
control_points.back().pose.position.y);
|
||||
else
|
||||
end_yaw = calculateAngle(control_points.back().pose.position.x,
|
||||
control_points.back().pose.position.y,
|
||||
saved_poses.back().pose.position.x,
|
||||
saved_poses.back().pose.position.y);
|
||||
end_yaw = normalizeAngle(end_yaw);
|
||||
|
||||
tf3::Quaternion q;
|
||||
q.setRPY(0, 0, end_yaw);
|
||||
saved_poses.back().pose.orientation.x = q.x();
|
||||
saved_poses.back().pose.orientation.y = q.y();
|
||||
saved_poses.back().pose.orientation.z = q.z();
|
||||
saved_poses.back().pose.orientation.w = q.w();
|
||||
|
||||
nurbs_path.push_back(saved_poses.back());
|
||||
}
|
||||
return nurbs_path;
|
||||
}
|
||||
|
||||
double MergePathCalc::calculateNURBSLength(Curve_common* curve_design,
|
||||
Spline_Inf* spline_inf,
|
||||
double initial_step)
|
||||
{
|
||||
double length = 0.0;
|
||||
geometry_msgs::Point prev_point, curr_point;
|
||||
std::vector<double> segment_lengths;
|
||||
|
||||
static double step;
|
||||
// Đường cong bậc 2 trở lên thì bước nhảy bằng initial_step
|
||||
step = initial_step;
|
||||
|
||||
// Tính độ dài với step
|
||||
for(double u = 0; u <= 1; u += step)
|
||||
{
|
||||
// Lấy điểm trên đường cong tại tham số u
|
||||
curr_point = curve_design->CalculateCurvePoint(spline_inf, u, true);
|
||||
|
||||
if(u > 0 && !std::isnan(curr_point.x) && !std::isnan(curr_point.y))
|
||||
{
|
||||
double segment_length = std::sqrt(std::pow(curr_point.x - prev_point.x, 2) +
|
||||
std::pow(curr_point.y - prev_point.y, 2));
|
||||
length += segment_length;
|
||||
segment_lengths.push_back(segment_length);
|
||||
}
|
||||
prev_point = curr_point;
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
void MergePathCalc::updatePoseOrientation(std::vector<geometry_msgs::PoseStamped>& saved_poses,
|
||||
std::vector<geometry_msgs::PoseStamped>& nurbs_path,
|
||||
bool reverse)
|
||||
{
|
||||
double yaw;
|
||||
// ROS_WARN("[merge_path_calc] DEBUG: reverse(%x)", reverse);
|
||||
if(!reverse)
|
||||
yaw = calculateAngle(saved_poses[saved_poses.size() - 2].pose.position.x,
|
||||
saved_poses[saved_poses.size() - 2].pose.position.y,
|
||||
saved_poses.back().pose.position.x,
|
||||
saved_poses.back().pose.position.y);
|
||||
else
|
||||
yaw = calculateAngle(saved_poses.back().pose.position.x,
|
||||
saved_poses.back().pose.position.y,
|
||||
saved_poses[saved_poses.size() - 2].pose.position.x,
|
||||
saved_poses[saved_poses.size() - 2].pose.position.y);
|
||||
|
||||
yaw = normalizeAngle(yaw);
|
||||
tf3::Quaternion q;
|
||||
q.setRPY(0, 0, yaw);
|
||||
saved_poses[saved_poses.size() - 2].pose.orientation.x = q.x();
|
||||
saved_poses[saved_poses.size() - 2].pose.orientation.y = q.y();
|
||||
saved_poses[saved_poses.size() - 2].pose.orientation.z = q.z();
|
||||
saved_poses[saved_poses.size() - 2].pose.orientation.w = q.w();
|
||||
|
||||
nurbs_path.push_back(saved_poses[saved_poses.size() - 2]);
|
||||
}
|
||||
|
||||
int MergePathCalc::findNearestPointOnPath(Pose& pose,
|
||||
std::vector<Pose>& posesOnPathWay,
|
||||
point_type type)
|
||||
{
|
||||
// Input validation - FIX: Thêm validation cơ bản
|
||||
if (posesOnPathWay.empty()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Các hằng số chung cho cả START và GOAL
|
||||
static const double MAX_DISTANCE = 1.0; // Ngưỡng khoảng cách tối đa (m)
|
||||
static const double MIN_DISTANCE = 0.1; // Khoảng cách tối thiểu (m)
|
||||
static const double MIN_SCALE = 0.2; // Giá trị tối thiểu của hệ số
|
||||
static const double MAX_SCALE = 1.0; // Giá trị tối đa của hệ số
|
||||
static const double MAX_ANGLE_THRESHOLD = 0.77 * M_PI; // Ngưỡng góc tối đa
|
||||
|
||||
int nearest_idx = 0;
|
||||
|
||||
if(type == START)
|
||||
{
|
||||
// FIX: Validate edge_front_end_idx_ trước khi sử dụng
|
||||
if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast<int>(posesOnPathWay.size())) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// === BƯỚC 1: TÌM ĐIỂM GẦN NHẤT CƠ BẢN ===
|
||||
double min_dist = std::numeric_limits<double>::max();
|
||||
double distance;
|
||||
|
||||
for (int i = 0; i < edge_front_end_idx_; ++i)
|
||||
{
|
||||
distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(),
|
||||
posesOnPathWay[i].getY() - pose.getY());
|
||||
|
||||
if (distance < min_dist)
|
||||
{
|
||||
min_dist = distance;
|
||||
nearest_idx = i;
|
||||
}
|
||||
}
|
||||
|
||||
// Lưu chỉ mục của điểm gần nhất
|
||||
store_start_nearest_idx_ = nearest_idx;
|
||||
|
||||
// === BƯỚC 2: TÍNH TOÁN HỆ SỐ SCALE ĐỘNG ===
|
||||
// Nội suy tuyến tính ngược: khoảng cách giảm → scale tăng
|
||||
double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) *
|
||||
(MAX_DISTANCE - std::min(min_dist, MAX_DISTANCE)) /
|
||||
(MAX_DISTANCE - MIN_DISTANCE);
|
||||
|
||||
// === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC ===
|
||||
double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI;
|
||||
if(angle_threshold >= MAX_ANGLE_THRESHOLD)
|
||||
angle_threshold = MAX_ANGLE_THRESHOLD;
|
||||
|
||||
// === BƯỚC 4: TÌM ĐIỂM THỎA MÃN ĐIỀU KIỆN GÓC ===
|
||||
for(int i = store_start_nearest_idx_; i <= edge_front_end_idx_; i++)
|
||||
{
|
||||
// Bounds checking đầy đủ trước khi truy cập
|
||||
if (i + 1 < static_cast<int>(posesOnPathWay.size()) && i + 1 <= edge_front_end_idx_)
|
||||
{
|
||||
double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i + 1]);
|
||||
|
||||
if(std::abs(angle) >= angle_threshold)
|
||||
{
|
||||
nearest_idx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if(i >= edge_front_end_idx_ - 1)
|
||||
{
|
||||
// Đã đến cuối mà không tìm thấy điểm phù hợp
|
||||
return -1; // Không tìm thấy điểm phù hợp
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(type == GOAL)
|
||||
{
|
||||
// FIX: Validate edge_back_start_idx_ trước khi sử dụng
|
||||
if (edge_back_start_idx_ < 0 || edge_back_start_idx_ >= static_cast<int>(posesOnPathWay.size())) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// === BƯỚC 1: TÌM ĐIỂM GẦN NHẤT CƠ BẢN (DUYỆT NGƯỢC) ===
|
||||
double min_dist = std::numeric_limits<double>::max();
|
||||
|
||||
for (int i = static_cast<int>(posesOnPathWay.size()) - 1; i >= edge_back_start_idx_; --i)
|
||||
{
|
||||
double distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(),
|
||||
posesOnPathWay[i].getY() - pose.getY());
|
||||
if (distance < min_dist)
|
||||
{
|
||||
min_dist = distance;
|
||||
nearest_idx = i;
|
||||
}
|
||||
}
|
||||
|
||||
// Lưu chỉ mục của điểm gần nhất
|
||||
store_goal_nearest_idx_ = nearest_idx;
|
||||
|
||||
// === BƯỚC 2: TÍNH TOÁN HỆ SỐ SCALE ĐỘNG (GIỐNG START) ===
|
||||
// Nội suy tuyến tính ngược: khoảng cách giảm → scale tăng
|
||||
double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) *
|
||||
(MAX_DISTANCE - std::min(min_dist, MAX_DISTANCE)) /
|
||||
(MAX_DISTANCE - MIN_DISTANCE);
|
||||
|
||||
// === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC (GIỐNG START) ===
|
||||
double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI;
|
||||
if(angle_threshold >= MAX_ANGLE_THRESHOLD)
|
||||
angle_threshold = MAX_ANGLE_THRESHOLD;
|
||||
|
||||
|
||||
// === BƯỚC 4: TÌM ĐIỂM THỎA MÃN ĐIỀU KIỆN GÓC (DUYỆT NGƯỢC) ===
|
||||
// Bắt đầu từ điểm đã tìm được, duyệt ngược để tìm điểm thỏa mãn góc
|
||||
for(int i = nearest_idx; i >= edge_back_start_idx_; i--)
|
||||
{
|
||||
// FIX: Bounds checking đầy đủ trước khi truy cập
|
||||
if (i - 1 >= edge_back_start_idx_ && i - 1 >= 0)
|
||||
{
|
||||
double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i - 1]);
|
||||
|
||||
if(std::abs(angle) >= angle_threshold)
|
||||
{
|
||||
nearest_idx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// FIX: Logic condition sửa lại để rõ ràng
|
||||
if(i <= edge_back_start_idx_ + 1)
|
||||
{
|
||||
// Đã đến đầu mà không tìm thấy điểm phù hợp
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
// FIX: Thêm else case để handle invalid type
|
||||
else {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return nearest_idx;
|
||||
}
|
||||
|
||||
bool MergePathCalc::findNURBSControlPoints(vector<geometry_msgs::PoseStamped>& control_points,
|
||||
const geometry_msgs::PoseStamped& pose,
|
||||
std::vector<Pose>& posesOnPathWay,
|
||||
point_type type)
|
||||
{
|
||||
// FIX: Input validation
|
||||
if (posesOnPathWay.empty()) return false;
|
||||
|
||||
// FIX: Chỉ clear một lần duy nhất thay vì clear nhiều lần
|
||||
control_points.clear();
|
||||
|
||||
if(type == START)
|
||||
{
|
||||
Pose start_pose;
|
||||
start_pose.setX(pose.pose.position.x);
|
||||
start_pose.setY(pose.pose.position.y);
|
||||
start_pose.setYaw(data_convert::getYaw(pose.pose.orientation));
|
||||
|
||||
int idx = findNearestPointOnPath(start_pose, posesOnPathWay, START);
|
||||
|
||||
// Trường hợp tạo đường thẳng
|
||||
if(idx == -1)
|
||||
{
|
||||
// FIX: Validate edge_front_end_idx_ trước khi sử dụng
|
||||
if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast<int>(posesOnPathWay.size())) {
|
||||
return false;
|
||||
}
|
||||
|
||||
start_target_idx_ = edge_front_end_idx_ - 1;
|
||||
|
||||
double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
|
||||
double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();
|
||||
|
||||
// Thêm điểm đầu
|
||||
control_points.push_back(pose);
|
||||
|
||||
// Với bậc 2, thêm 1 điểm trung gian nằm trên đường thẳng
|
||||
geometry_msgs::PoseStamped mid_pose;
|
||||
mid_pose.pose.position.x = start_pose.getX() + dx/2.0;
|
||||
mid_pose.pose.position.y = start_pose.getY() + dy/2.0;
|
||||
mid_pose.pose.orientation = pose.pose.orientation;
|
||||
// FIX: Copy header từ pose gốc
|
||||
mid_pose.header = pose.header;
|
||||
control_points.push_back(mid_pose);
|
||||
|
||||
// Thêm điểm cuối
|
||||
geometry_msgs::PoseStamped end_pose;
|
||||
end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
|
||||
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
|
||||
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
|
||||
// FIX: Copy header từ pose gốc
|
||||
end_pose.header = pose.header;
|
||||
control_points.push_back(end_pose);
|
||||
|
||||
return true;
|
||||
}
|
||||
// Tìm đường cong
|
||||
else
|
||||
{
|
||||
start_target_idx_ = idx;
|
||||
|
||||
// Tạo các control point cho NURBS
|
||||
control_points.push_back(pose);
|
||||
|
||||
geometry_msgs::PoseStamped mid_pose;
|
||||
mid_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
|
||||
mid_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
|
||||
mid_pose.pose.orientation = pose.pose.orientation;
|
||||
// FIX: Copy header từ pose gốc
|
||||
mid_pose.header = pose.header;
|
||||
control_points.push_back(mid_pose);
|
||||
|
||||
|
||||
double dx_sm = start_pose.getX() - posesOnPathWay[start_target_idx_].getX();
|
||||
double dy_sm = start_pose.getY() - posesOnPathWay[start_target_idx_].getY();
|
||||
double dist_sm = std::hypot(dx_sm,dy_sm);
|
||||
|
||||
// FIX: Bỏ biến end_idx_ không sử dụng
|
||||
// end_idx_ = start_target_idx_;
|
||||
|
||||
bool found_suitable_point = false; // FIX: Thêm flag để tránh goto
|
||||
for(int i = start_target_idx_; i <= edge_front_end_idx_; i++)
|
||||
{
|
||||
// FIX: Validate bounds trước khi truy cập
|
||||
if (i >= static_cast<int>(posesOnPathWay.size())) {
|
||||
break;
|
||||
}
|
||||
|
||||
double dx_me = posesOnPathWay[i].getX() - posesOnPathWay[start_target_idx_].getX();
|
||||
double dy_me = posesOnPathWay[i].getY() - posesOnPathWay[start_target_idx_].getY();
|
||||
double dist_me = std::hypot(dx_me,dy_me);
|
||||
if(dist_me >= dist_sm)
|
||||
{
|
||||
start_target_idx_ = i;
|
||||
found_suitable_point = true;
|
||||
break;
|
||||
}
|
||||
if(i == edge_front_end_idx_)
|
||||
{
|
||||
// FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
|
||||
found_suitable_point = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// FIX: Xử lý trường hợp không tìm thấy điểm phù hợp bằng cách tạo straight line
|
||||
if (!found_suitable_point) {
|
||||
// Clear và tạo straight line thay vì goto
|
||||
control_points.clear();
|
||||
|
||||
// FIX: Validate lại edge_front_end_idx_
|
||||
if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast<int>(posesOnPathWay.size())) {
|
||||
return false;
|
||||
}
|
||||
|
||||
start_target_idx_ = edge_front_end_idx_ - 1;
|
||||
|
||||
double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
|
||||
double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();
|
||||
|
||||
control_points.push_back(pose);
|
||||
|
||||
geometry_msgs::PoseStamped mid_pose_straight;
|
||||
mid_pose_straight.pose.position.x = start_pose.getX() + dx/2.0;
|
||||
mid_pose_straight.pose.position.y = start_pose.getY() + dy/2.0;
|
||||
mid_pose_straight.pose.orientation = pose.pose.orientation;
|
||||
mid_pose_straight.header = pose.header;
|
||||
control_points.push_back(mid_pose_straight);
|
||||
|
||||
geometry_msgs::PoseStamped end_pose_straight;
|
||||
end_pose_straight.pose.position.x = posesOnPathWay[start_target_idx_].getX();
|
||||
end_pose_straight.pose.position.y = posesOnPathWay[start_target_idx_].getY();
|
||||
end_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
|
||||
end_pose_straight.header = pose.header;
|
||||
control_points.push_back(end_pose_straight);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
geometry_msgs::PoseStamped end_pose;
|
||||
end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
|
||||
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
|
||||
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
|
||||
// FIX: Copy header từ pose gốc
|
||||
end_pose.header = pose.header;
|
||||
control_points.push_back(end_pose);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
else if(type == GOAL)
|
||||
{
|
||||
Pose goal_pose;
|
||||
goal_pose.setX(pose.pose.position.x);
|
||||
goal_pose.setY(pose.pose.position.y);
|
||||
goal_pose.setYaw(data_convert::getYaw(pose.pose.orientation));
|
||||
|
||||
int idx = findNearestPointOnPath(goal_pose, posesOnPathWay, GOAL);
|
||||
|
||||
if(idx == -1)
|
||||
{
|
||||
// Trường hợp tạo đường thẳng
|
||||
// FIX: Validate size trước khi sử dụng
|
||||
if (posesOnPathWay.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
|
||||
|
||||
// Thêm điểm đầu từ đường đã có
|
||||
geometry_msgs::PoseStamped start_pose;
|
||||
start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
|
||||
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
|
||||
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
|
||||
// FIX: Copy header từ pose gốc
|
||||
start_pose.header = pose.header;
|
||||
control_points.push_back(start_pose);
|
||||
|
||||
// Vector hướng từ goal tới start
|
||||
double dx = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
|
||||
double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
|
||||
|
||||
// Thêm 1 điểm trung gian cho bậc 2
|
||||
geometry_msgs::PoseStamped mid_pose;
|
||||
mid_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx/2.0;
|
||||
mid_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy/2.0;
|
||||
mid_pose.pose.orientation = start_pose.pose.orientation;
|
||||
// FIX: Copy header từ pose gốc
|
||||
mid_pose.header = pose.header;
|
||||
control_points.push_back(mid_pose);
|
||||
|
||||
// Thêm điểm goal
|
||||
control_points.push_back(pose);
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Trường hợp tạo đường cong
|
||||
goal_target_idx_ = idx;
|
||||
|
||||
double dx = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
|
||||
double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
|
||||
double dist_me = std::hypot(dx, dy);
|
||||
|
||||
bool found_suitable_point = false; // FIX: Thêm flag để tránh goto
|
||||
for(int i = goal_target_idx_; i >= edge_back_start_idx_; i--)
|
||||
{
|
||||
// FIX: Validate bounds trước khi truy cập
|
||||
if (i < 0 || i >= static_cast<int>(posesOnPathWay.size())) {
|
||||
break;
|
||||
}
|
||||
|
||||
double dx_sm = posesOnPathWay[i].getX() - posesOnPathWay[goal_target_idx_].getX();
|
||||
double dy_sm = posesOnPathWay[i].getY() - posesOnPathWay[goal_target_idx_].getY();
|
||||
double dist_sm = std::hypot(dx_sm,dy_sm);
|
||||
if(dist_sm >= dist_me)
|
||||
{
|
||||
goal_target_idx_ = i;
|
||||
found_suitable_point = true;
|
||||
break;
|
||||
}
|
||||
if(i == edge_back_start_idx_)
|
||||
{
|
||||
// FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
|
||||
found_suitable_point = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// FIX: Xử lý trường hợp không tìm thấy điểm phù hợp bằng cách tạo straight line
|
||||
if (!found_suitable_point) {
|
||||
// Clear và tạo straight line thay vì goto
|
||||
control_points.clear();
|
||||
|
||||
// FIX: Validate lại size
|
||||
if (posesOnPathWay.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
|
||||
|
||||
geometry_msgs::PoseStamped start_pose_straight;
|
||||
start_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
|
||||
start_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
|
||||
start_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
|
||||
start_pose_straight.header = pose.header;
|
||||
control_points.push_back(start_pose_straight);
|
||||
|
||||
double dx_straight = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
|
||||
double dy_straight = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
|
||||
|
||||
geometry_msgs::PoseStamped mid_pose_straight;
|
||||
mid_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx_straight/2.0;
|
||||
mid_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy_straight/2.0;
|
||||
mid_pose_straight.pose.orientation = start_pose_straight.pose.orientation;
|
||||
mid_pose_straight.header = pose.header;
|
||||
control_points.push_back(mid_pose_straight);
|
||||
|
||||
control_points.push_back(pose);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Thêm điểm đầu từ đường đã có
|
||||
geometry_msgs::PoseStamped start_pose;
|
||||
start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
|
||||
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
|
||||
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
|
||||
// FIX: Copy header từ pose gốc
|
||||
start_pose.header = pose.header;
|
||||
control_points.push_back(start_pose);
|
||||
|
||||
geometry_msgs::PoseStamped mid_pose;
|
||||
mid_pose.pose.position.x = posesOnPathWay[idx].getX();
|
||||
mid_pose.pose.position.y = posesOnPathWay[idx].getY();
|
||||
mid_pose.pose.orientation = pose.pose.orientation;
|
||||
// FIX: Copy header từ pose gốc
|
||||
mid_pose.header = pose.header;
|
||||
control_points.push_back(mid_pose);
|
||||
|
||||
// Thêm điểm goal
|
||||
control_points.push_back(pose);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
// FIX: Thêm else case để handle invalid type
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
|
||||
// FIX: Code không bao giờ reach được đây, nhưng để đảm bảo
|
||||
// return false; // Không tìm thấy control points
|
||||
}
|
||||
|
||||
double MergePathCalc::signedAngle(Pose& pose, std::vector<Pose>& posesOnPathWay,
|
||||
point_type type, int& idx)
|
||||
{
|
||||
if(type == START)
|
||||
{
|
||||
// Vector AB
|
||||
double bax = pose.getX() - posesOnPathWay[idx].getX();
|
||||
double bay = pose.getY() - posesOnPathWay[idx].getY();
|
||||
|
||||
// Vector BC
|
||||
double bcx = posesOnPathWay[idx+1].getX() - posesOnPathWay[idx].getX();
|
||||
double bcy = posesOnPathWay[idx+1].getY() - posesOnPathWay[idx].getY();
|
||||
|
||||
// Tích vô hướng
|
||||
double dot = bax * bcx + bay * bcy;
|
||||
// Tích hướng 2D (z-component)
|
||||
double cross = bax * bcy - bay * bcx;
|
||||
|
||||
// Góc có hướng [-π, π]
|
||||
double angle_rad = std::atan2(cross, dot);
|
||||
return angle_rad;
|
||||
}
|
||||
else if(type == GOAL)
|
||||
{
|
||||
// Vector AB
|
||||
double bax = posesOnPathWay[idx-1].getX() - posesOnPathWay[idx].getX();
|
||||
double bay = posesOnPathWay[idx-1].getY() - posesOnPathWay[idx].getY();
|
||||
|
||||
// Vector BC
|
||||
double bcx = pose.getX() - posesOnPathWay[idx].getX();
|
||||
double bcy = pose.getY() - posesOnPathWay[idx].getY();
|
||||
|
||||
// Tích vô hướng
|
||||
double dot = bax * bcx + bay * bcy;
|
||||
// Tích hướng 2D (z-component)
|
||||
double cross = bax * bcy - bay * bcx;
|
||||
|
||||
// Góc có hướng [-π, π]
|
||||
double angle_rad = std::atan2(cross, dot);
|
||||
return angle_rad;
|
||||
}
|
||||
else return -1; // Trả về -1 nếu không phải START hoặc GOAL
|
||||
|
||||
}
|
||||
|
||||
double MergePathCalc::signedAngle(Pose& start_pose, Pose& mid_pose, Pose& end_pose)
|
||||
{
|
||||
// Vector AB
|
||||
double bax = mid_pose.getX() - start_pose.getX();
|
||||
double bay = mid_pose.getY() - start_pose.getY();
|
||||
|
||||
// Vector BC
|
||||
double bcx = mid_pose.getX() - end_pose.getX();
|
||||
double bcy = mid_pose.getY() - end_pose.getY();
|
||||
|
||||
// Tích vô hướng
|
||||
double dot = bax * bcx + bay * bcy;
|
||||
// Tích hướng 2D (z-component)
|
||||
double cross = bax * bcy - bay * bcx;
|
||||
|
||||
// Góc có hướng [-π, π]
|
||||
double angle_rad = std::atan2(cross, dot);
|
||||
return angle_rad;
|
||||
}
|
||||
|
||||
} // namespace custom_planner
|
||||
291
src/pathway.cc
Executable file
291
src/pathway.cc
Executable file
|
|
@ -0,0 +1,291 @@
|
|||
#include "custom_planner/pathway.h"
|
||||
|
||||
using namespace custom_planner;
|
||||
|
||||
bool Pathway::onSegment(Pose p, Pose q, Pose r)
|
||||
{
|
||||
if (q.getX() <= max(p.getX(), r.getX()) && q.getX() >= min(p.getX(), r.getX()) &&
|
||||
q.getY() <= max(p.getY(), r.getY()) && q.getY() >= min(p.getY(), r.getY()))
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int Pathway::orientation(Pose p, Pose q, Pose r)
|
||||
{
|
||||
int val = (q.getY() - p.getY()) * (r.getX() - q.getX()) -
|
||||
(q.getX() - p.getX()) * (r.getY() - q.getY());
|
||||
|
||||
if (val == 0) return 0; // collinear
|
||||
|
||||
return (val > 0)? 1: 2; // clock or counterclock wise
|
||||
}
|
||||
|
||||
bool Pathway::doIntersect(Pose p1, Pose q1, Pose p2, Pose q2)
|
||||
{
|
||||
// Find the four orientations needed for general and
|
||||
// special cases
|
||||
int o1 = orientation(p1, q1, p2);
|
||||
int o2 = orientation(p1, q1, q2);
|
||||
int o3 = orientation(p2, q2, p1);
|
||||
int o4 = orientation(p2, q2, q1);
|
||||
|
||||
// General case
|
||||
if (o1 != o2 && o3 != o4)
|
||||
return true;
|
||||
|
||||
// Special Cases
|
||||
// p1, q1 and p2 are collinear and p2 lies on segment p1q1
|
||||
if (o1 == 0 && onSegment(p1, p2, q1)) return true;
|
||||
|
||||
// p1, q1 and q2 are collinear and q2 lies on segment p1q1
|
||||
if (o2 == 0 && onSegment(p1, q2, q1)) return true;
|
||||
|
||||
// p2, q2 and p1 are collinear and p1 lies on segment p2q2
|
||||
if (o3 == 0 && onSegment(p2, p1, q2)) return true;
|
||||
|
||||
// p2, q2 and q1 are collinear and q1 lies on segment p2q2
|
||||
if (o4 == 0 && onSegment(p2, q1, q2)) return true;
|
||||
|
||||
return false; // Doesn't fall in any of the above cases
|
||||
}
|
||||
|
||||
bool Pathway::isIntersect(Edge L) {
|
||||
for (auto& segment : path_) {
|
||||
if (doIntersect(L.getP1(), L.getP2(), segment.getP1(), segment.getP2())) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool Pathway::isPoseExisted(Pose p)
|
||||
{
|
||||
for(auto& pose : posesOnPath_)
|
||||
{
|
||||
if(p.getX()==pose.getX() && p.getY()==pose.getY()) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
double Pathway::calculateAngle(Pose p1, Pose p2, Pose p3)
|
||||
{
|
||||
// Tính vector 1
|
||||
double vector1x = p2.getX() - p1.getX();
|
||||
double vector1y = p2.getY() - p1.getY();
|
||||
|
||||
// Tính vector 2
|
||||
double vector2x = p3.getX() - p2.getX();
|
||||
double vector2y = p3.getY() - p2.getY();
|
||||
|
||||
// Tính độ dài của hai vector
|
||||
double lengthVector1 = sqrt(vector1x * vector1x + vector1y * vector1y);
|
||||
double lengthVector2 = sqrt(vector2x * vector2x + vector2y * vector2y);
|
||||
|
||||
// Tính góc giữa hai vector
|
||||
double dotProduct = vector1x * vector2x + vector1y * vector2y;
|
||||
double cosTheta = dotProduct / (lengthVector1 * lengthVector2);
|
||||
if(cosTheta>1) cosTheta = 1;
|
||||
else if(cosTheta<(-1)) cosTheta = -1;
|
||||
// Đổi radian sang độ
|
||||
double angle = acos(cosTheta) * 180.0 / M_PI;
|
||||
|
||||
return angle;
|
||||
}
|
||||
void Pathway::RecordPath(Pose startPose, Pose currentPose, double delta_angle_th, double InitialSegmentLengthMin, double SegmentLengthMin)
|
||||
{
|
||||
if(!isPathInitilized_)
|
||||
{
|
||||
double InitialSegmentLength = sqrt(pow(currentPose.getX() - startPose.getX(), 2) + pow(currentPose.getY() - startPose.getY(), 2));
|
||||
if(InitialSegmentLength>=InitialSegmentLengthMin)
|
||||
{
|
||||
addEdge(startPose, currentPose);
|
||||
isPathInitilized_ = true;
|
||||
printf("path is initialized\n");
|
||||
}
|
||||
|
||||
}
|
||||
if(!path_.empty())
|
||||
{
|
||||
double distanceCheck = sqrt(pow(currentPose.getX() - path_.back().getP2().getX(), 2) + pow(currentPose.getY() - path_.back().getP2().getY(), 2));
|
||||
if(distanceCheck>=SegmentLengthMin)
|
||||
{
|
||||
if(isNewPoseOnEdge(currentPose, delta_angle_th)){
|
||||
setP2LastEdgeOnPath(currentPose);
|
||||
printf("add new pose on segment into path\n");
|
||||
}
|
||||
else{
|
||||
addEdge(currentPose);
|
||||
printf("add new segment into path\n");
|
||||
}
|
||||
printf("Number of segment on path: %d\n",(int)path_.size());
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Pathway::ResetAndRecordPath(Pose startPose, Pose currentPose, double delta_angle_th, double InitialSegmentLengthMin, double SegmentLengthMin)
|
||||
{
|
||||
if(!isPathClear_){
|
||||
path_.clear();
|
||||
isPathClear_ = true;
|
||||
}
|
||||
RecordPath(startPose, currentPose, delta_angle_th, InitialSegmentLengthMin, SegmentLengthMin);
|
||||
}
|
||||
|
||||
bool Pathway::isNewPoseOnEdge(Pose p, double delta_angle_th)
|
||||
{
|
||||
if(!path_.empty())
|
||||
{
|
||||
double deltaAngle = calculateAngle(path_.back().getP1(), path_.back().getP2(), p);
|
||||
// ROS_WARN("deltaAngle: %f",deltaAngle);
|
||||
if(deltaAngle<=delta_angle_th) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void Pathway::setEdgeOnPath(uint32_t line_index, Pose p1, Pose p2){
|
||||
if(!path_.empty()&&line_index<=path_.size())
|
||||
{
|
||||
path_.at(line_index).setEdge(p1, p2);
|
||||
}
|
||||
}
|
||||
|
||||
void Pathway::syncPosesAndPath()
|
||||
{
|
||||
if(!path_.empty())
|
||||
{
|
||||
posesOnPath_.clear();
|
||||
for(auto& edge : path_)
|
||||
{
|
||||
if(!isPoseExisted(edge.getP1())) posesOnPath_.push_back(edge.getP1());
|
||||
if(!isPoseExisted(edge.getP2())) posesOnPath_.push_back(edge.getP2());
|
||||
}
|
||||
}
|
||||
else posesOnPath_.clear();
|
||||
}
|
||||
|
||||
void Pathway::SavePathAsFile(const string& file_name)
|
||||
{
|
||||
ofstream file(file_name);
|
||||
|
||||
if (file.is_open())
|
||||
{
|
||||
for (auto &edge : path_)
|
||||
{
|
||||
file << edge.getP1().getX() << " " << edge.getP1().getY() << " " << edge.getP1().getYaw()
|
||||
<< edge.getP2().getX() << " " << edge.getP2().getY() << " " << edge.getP2().getYaw() << "\n";
|
||||
}
|
||||
|
||||
file.close();
|
||||
cout << "Pathway saved to file: " <<file_name<< endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
cerr << "Unable to open file for saving: "<<file_name<< endl;
|
||||
}
|
||||
}
|
||||
|
||||
void Pathway::LoadPathFromFile(const string& file_name)
|
||||
{
|
||||
ifstream file(file_name);
|
||||
|
||||
if (file.is_open())
|
||||
{
|
||||
path_.clear();
|
||||
|
||||
double x1, y1, x2, y2, yaw1, yaw2;
|
||||
|
||||
while (file >> x1 >> y1 >> yaw1 >> x2 >> y2 >> yaw2)
|
||||
{
|
||||
Pose p1(x1, y1, yaw1);
|
||||
Pose p2(x2, y2, yaw2);
|
||||
|
||||
addEdge(p1, p2);
|
||||
}
|
||||
|
||||
file.close();
|
||||
cout << "Pathway loaded from file: " <<file_name<< endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
cerr << "Unable to open file for loading: " <<file_name<< endl;
|
||||
}
|
||||
}
|
||||
|
||||
bool Pathway::findIntersection(Edge L1, Edge L2, Point* resultPoint)
|
||||
{
|
||||
/*
|
||||
A = (x1, y1)
|
||||
B = (x2, y2)
|
||||
C = (x3, y3)
|
||||
D = (x4, y4)
|
||||
AB = (x2 - x1, y2 - y1)
|
||||
CD = (x4 - x3, y4 - y3)
|
||||
A + αAB = C + βCD
|
||||
α = ((x4 - x3)*(y3 - y1) - (y4 - y3)*(x3 - x1)) / ((x4 - x3)*(y2 - y1) - (y4 - y3)*(x2 - x1)) = a / b
|
||||
β = ((x2 - x1)*(y3 - y1) - (y2 - y1)*(x3 - x1)) / ((x4 - x3)*(y2 - y1) - (y4 - y3)*(x2 - x1)) = c / b
|
||||
P = (x0, y0)
|
||||
x0 = x1 + α*(x2 - x1) = x3 + β*(x4 - x3)
|
||||
y0 = y1 + α*(y2 - y1) = y3 + β*(y4 - y3)
|
||||
*/
|
||||
double x1 = L1.getP1().getX();
|
||||
double y1 = L1.getP1().getY();
|
||||
double x2 = L1.getP2().getX();
|
||||
double y2 = L1.getP2().getY();
|
||||
double x3 = L2.getP1().getX();
|
||||
double y3 = L2.getP1().getY();
|
||||
double x4 = L2.getP2().getX();
|
||||
double y4 = L2.getP2().getY();
|
||||
double a = (x4 - x3)*(y3 - y1) - (y4 - y3)*(x3 - x1);
|
||||
double b = (x4 - x3)*(y2 - y1) - (y4 - y3)*(x2 - x1);
|
||||
double c = (x2 - x1)*(y3 - y1) - (y2 - y1)*(x3 - x1);
|
||||
double alpha = a/b;
|
||||
double beta = c/b;
|
||||
if(b==0) return false;
|
||||
if(a==0&&b==0) return false;
|
||||
else if(alpha>=0&&alpha<=1&&beta>=0&&beta<=1)
|
||||
{
|
||||
double x0 = x1 + alpha*(x2 - x1);
|
||||
double y0 = y1 + alpha*(y2 - y1);
|
||||
resultPoint->setPoint(x0,y0);
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
vector<Point> Pathway::findIntersections(Edge L) {
|
||||
vector<Point> intersectionPoints;
|
||||
if(!path_.empty()){
|
||||
for (auto& segment : path_){
|
||||
Point result;
|
||||
if(findIntersection(L,segment,&result)==true)
|
||||
{
|
||||
intersectionPoints.push_back(result);
|
||||
}
|
||||
}
|
||||
}
|
||||
return intersectionPoints;
|
||||
}
|
||||
|
||||
vector<vector<Point>> Pathway::findIntersections(Edge L1, Edge L2) {
|
||||
vector<vector<Point>> intersectionPoints;
|
||||
vector<Point> intersectionPointsL1;
|
||||
vector<Point> intersectionPointsL2;
|
||||
if(!path_.empty()){
|
||||
for (auto& segment : path_) {
|
||||
Point result1;
|
||||
if(findIntersection(L1,segment,&result1)==true)
|
||||
{
|
||||
intersectionPointsL1.push_back(result1);
|
||||
}
|
||||
Point result2;
|
||||
if(findIntersection(L2,segment,&result2)==true)
|
||||
{
|
||||
intersectionPointsL2.push_back(result2);
|
||||
}
|
||||
}
|
||||
}
|
||||
intersectionPoints.push_back(intersectionPointsL1);
|
||||
intersectionPoints.push_back(intersectionPointsL2);
|
||||
|
||||
return intersectionPoints;
|
||||
}
|
||||
686
src/plan_for_retry.cpp
Executable file
686
src/plan_for_retry.cpp
Executable file
|
|
@ -0,0 +1,686 @@
|
|||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <ros/ros.h>
|
||||
#include <tf/tf.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
inline double calculateAngle(double xA, double yA, double xB, double yB) {
|
||||
double deltaX = xB - xA;
|
||||
double deltaY = yB - yA;
|
||||
double angleRad = 0;
|
||||
if(deltaX!=0)
|
||||
{
|
||||
angleRad = atan2(deltaY, deltaX);
|
||||
// double angleDeg = angleRad * 180.0 / M_PI;
|
||||
}
|
||||
return angleRad;
|
||||
}
|
||||
|
||||
inline double getYaw(double x, double y, double z, double w){
|
||||
// yaw (z-axis rotation)
|
||||
double siny_cosp = 2 * (w * z + x * y);
|
||||
double cosy_cosp = 1 - 2 * (y * y + z * z);
|
||||
double yaw = std::atan2(siny_cosp, cosy_cosp);
|
||||
return yaw;
|
||||
}
|
||||
|
||||
bool isThetaValid(double theta)
|
||||
{
|
||||
bool result = false;
|
||||
if(theta < -M_PI || theta > M_PI) result = false;
|
||||
else result = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
if(isThetaValid(theta))
|
||||
{
|
||||
double xAB = next_Pose.position.x - startPose.position.x;
|
||||
double yAB = next_Pose.position.y - startPose.position.y;
|
||||
double d = sqrt(xAB*xAB + yAB*yAB);
|
||||
double xC = startPose.position.x + d*cos(theta);
|
||||
double yC = startPose.position.y + d*sin(theta);
|
||||
double xAC = xC-startPose.position.x;
|
||||
double yAC = yC-startPose.position.y;
|
||||
double dAB = sqrt(xAB*xAB + yAB*yAB);
|
||||
double cos_a = (xAB*xAC + yAB*yAC)/(dAB*d);
|
||||
if(cos_a>1) cos_a = 1;
|
||||
else if(cos_a<(-1)) cos_a = -1;
|
||||
delta_angle = acos(cos_a);
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("xC: %f, yC: %f", xC, yC);
|
||||
// ROS_WARN("dAB: %f", dAB);
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
}
|
||||
return delta_angle;
|
||||
}
|
||||
|
||||
double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
if(isThetaValid(theta))
|
||||
{
|
||||
double xAB =endPose.position.x-prev_Pose.position.x;
|
||||
double yAB = endPose.position.y-prev_Pose.position.y;
|
||||
double d = sqrt(xAB*xAB + yAB*yAB);
|
||||
double xC =endPose.position.x + d*cos(theta);
|
||||
double yC = endPose.position.y + d*sin(theta);
|
||||
double xBC = xC-endPose.position.x;
|
||||
double yBC = yC-endPose.position.y;
|
||||
double dAB = sqrt(xAB*xAB + yAB*yAB);
|
||||
double cos_a = (xAB*xBC + yAB*yBC)/(dAB*d);
|
||||
if(cos_a>1) cos_a = 1;
|
||||
else if(cos_a<(-1)) cos_a = -1;
|
||||
delta_angle = acos(cos_a);
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("xC: %f, yC: %f", xC, yC);
|
||||
// ROS_WARN("dAB: %f", dAB);
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
}
|
||||
return delta_angle;
|
||||
}
|
||||
|
||||
// Hàm chia đoạn thẳng AB thành các đoạn có độ dài d
|
||||
std::vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped& A, geometry_msgs::PoseStamped& B, double d) {
|
||||
std::vector<geometry_msgs::PoseStamped> Poses;
|
||||
double xAB = B.pose.position.x - A.pose.position.x;
|
||||
double yAB = B.pose.position.y - A.pose.position.y;
|
||||
double length = sqrt(xAB*xAB + yAB*yAB);
|
||||
if(length > d)
|
||||
{
|
||||
Poses.push_back(A); // Thêm điểm A vào vector trước khi chia
|
||||
|
||||
int segments = length / d;
|
||||
|
||||
// Tính toán tọa độ của các điểm trên đoạn AB
|
||||
double ratio = d / length;
|
||||
for (int i = 1; i <= segments; ++i) {
|
||||
geometry_msgs::PoseStamped p;
|
||||
double p_x = A.pose.position.x + (B.pose.position.x - A.pose.position.x) * ratio * i;
|
||||
double p_y = A.pose.position.y + (B.pose.position.y - A.pose.position.y) * ratio * i;
|
||||
p.pose.position.x = p_x;
|
||||
p.pose.position.y = p_y;
|
||||
Poses.push_back(p);
|
||||
}
|
||||
|
||||
if(!Poses.empty()&&(Poses.back().pose.position.x!=B.pose.position.x || Poses.back().pose.position.y!=B.pose.position.y))
|
||||
{
|
||||
Poses.push_back(B); // Thêm điểm B vào vector sau khi chia
|
||||
}
|
||||
|
||||
// Tính góc cho từng pose trên đoạn AB
|
||||
if(//computeDeltaAngleStartOfPlan(getYaw(A.pose.orientation.x, A.pose.orientation.y, A.pose.orientation.z, A.pose.orientation.w),
|
||||
//Poses.front().pose, Poses[1].pose) <= 0.872664626 &&
|
||||
computeDeltaAngleEndOfPlan(getYaw(B.pose.orientation.x, B.pose.orientation.y, B.pose.orientation.z, B.pose.orientation.w),
|
||||
Poses.back().pose, Poses[Poses.size() - 2].pose) <= 1.3962634016) // <= 80 degree
|
||||
{
|
||||
double theta_tmp = 0;
|
||||
for(int i = 0; i<((int)Poses.size()-1); i++)
|
||||
{
|
||||
if(Poses[i].pose.position.x!=Poses[i+1].pose.position.x)
|
||||
{
|
||||
double theta = calculateAngle(Poses[i].pose.position.x, Poses[i].pose.position.y,
|
||||
Poses[i+1].pose.position.x, Poses[i+1].pose.position.y);
|
||||
Poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta);
|
||||
theta_tmp = theta;
|
||||
}
|
||||
else
|
||||
{
|
||||
Poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp);
|
||||
}
|
||||
}
|
||||
Poses.back().pose.orientation = B.pose.orientation;
|
||||
}
|
||||
else if(//computeDeltaAngleStartOfPlan(getYaw(A.pose.orientation.x, A.pose.orientation.y, A.pose.orientation.z, A.pose.orientation.w),
|
||||
//Poses.front().pose, Poses[1].pose) >= 2.2689280276 &&
|
||||
computeDeltaAngleEndOfPlan(getYaw(B.pose.orientation.x, B.pose.orientation.y, B.pose.orientation.z, B.pose.orientation.w),
|
||||
Poses.back().pose, Poses[Poses.size() - 2].pose) >= 1.7453292526) // >= 100 degree
|
||||
{
|
||||
double theta_tmp = 0;
|
||||
for(int i = (int)Poses.size() -1; i>0; i--)
|
||||
{
|
||||
if(Poses[i].pose.position.x!=Poses[i-1].pose.position.x)
|
||||
{
|
||||
double theta = calculateAngle(Poses[i].pose.position.x, Poses[i].pose.position.y,
|
||||
Poses[i-1].pose.position.x, Poses[i-1].pose.position.y);
|
||||
Poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta);
|
||||
theta_tmp = theta;
|
||||
}
|
||||
else
|
||||
{
|
||||
Poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp);
|
||||
}
|
||||
}
|
||||
Poses.front().pose.orientation = A.pose.orientation;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
Poses.push_back(A);
|
||||
Poses.push_back(B);
|
||||
}
|
||||
return Poses;
|
||||
}
|
||||
|
||||
// Hàm gọi make plan : tạo tuyến đường đi quay lại điểm retry có trong plan hiện tại và đi tới điểm goal mới theo cung tròn.
|
||||
// khi tạo thành công plan thì hàm trả về True, không thành công thì trả về False và có hiện cảnh báo nguyên nhân.
|
||||
// current_plan: vector chứa plan hiện tại
|
||||
// pose_A: điểm quay lại để retry
|
||||
// pose_B: điểm goal mới, có hướng thẳng với palet hiện tại
|
||||
// pose_C: tâm của cung tròn AB
|
||||
// result_plan: vector chứa plan kết quả
|
||||
|
||||
bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
|
||||
int indexOfPoseA, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_B_behind,
|
||||
geometry_msgs::PoseStamped& pose_C, std::vector<geometry_msgs::PoseStamped>& result_plan)
|
||||
{
|
||||
bool result = false;
|
||||
std::vector<geometry_msgs::PoseStamped> PlanRetry_1;
|
||||
std::vector<geometry_msgs::PoseStamped> PlanRetry_2;
|
||||
std::vector<geometry_msgs::PoseStamped> PlanRetry_3;
|
||||
|
||||
if(current_plan.empty()||current_plan.size()<2)
|
||||
{
|
||||
ROS_WARN("current_plan is empty");
|
||||
return false;
|
||||
}
|
||||
|
||||
geometry_msgs::PoseStamped pose_A;
|
||||
pose_A = current_plan[indexOfPoseA];
|
||||
|
||||
// Tính ra PlanRetry_1 điểm retry tại Pose_A
|
||||
|
||||
PlanRetry_1.assign(current_plan.begin()+indexOfPoseA, current_plan.end());
|
||||
|
||||
if(!PlanRetry_1.empty()){
|
||||
std::reverse(PlanRetry_1.begin(), PlanRetry_1.end());
|
||||
}
|
||||
|
||||
// Tính ra PlanRetry_2 với biên dạng cung tròn đi qua pose_A và pose_B, có tâm tại pose_C
|
||||
|
||||
double xAB = pose_B.pose.position.x - pose_A.pose.position.x;
|
||||
double yAB = pose_B.pose.position.y - pose_A.pose.position.y;
|
||||
double d_AB = sqrt(xAB*xAB + yAB*yAB);
|
||||
if(d_AB<=0.1)
|
||||
{
|
||||
ROS_WARN("Curve AB is too short, cannot compute plan");
|
||||
return false;
|
||||
}
|
||||
|
||||
// nếu hướng của vector AB và hướng của pose_B tạo với nhau một góc ~0 độ hoặc ~180 độ -> cung tròn AB sẽ gần như là một đọan thẳng
|
||||
if((computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) >= 3.13 &&
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) <= M_PI) ||
|
||||
(computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) <= 0.1745 &&
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) >= 0))
|
||||
{
|
||||
std::vector<geometry_msgs::PoseStamped> planSegment_AB;
|
||||
planSegment_AB = divideSegment(pose_A, pose_B, 0.1);
|
||||
PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end());
|
||||
}
|
||||
else
|
||||
{
|
||||
double xCA = pose_A.pose.position.x - pose_C.pose.position.x;
|
||||
double yCA = pose_A.pose.position.y - pose_C.pose.position.y;
|
||||
double xCB = pose_B.pose.position.x - pose_C.pose.position.x;
|
||||
double yCB = pose_B.pose.position.y - pose_C.pose.position.y;
|
||||
double rCA = sqrt(xCA*xCA + yCA*yCA);
|
||||
double rCB = sqrt(xCB*xCB + yCB*yCB);
|
||||
if(abs(rCA-rCB)>0.008)
|
||||
{
|
||||
ROS_WARN("pose_C is not Center of Curve AB");
|
||||
return false;
|
||||
}
|
||||
|
||||
double cos_ACB = (xCA*xCB + yCA*yCB)/(rCA*rCB);
|
||||
if(cos_ACB>1) cos_ACB = 1;
|
||||
else if(cos_ACB<(-1)) cos_ACB = -1;
|
||||
double angleACB = acos(cos_ACB);
|
||||
double angle_interval = 0.005;
|
||||
// tính góc của vector CA:
|
||||
double angleCA = atan2(yCA, xCA);
|
||||
|
||||
// check thử xem chiều góc quét từ A -> B thì angleCA + delta_angle hay angleCA - delta_angle
|
||||
bool is_increase_angle = false;
|
||||
double check_angle = angleCA + 50*angle_interval*angleACB;
|
||||
double xA1 = pose_C.pose.position.x + rCA*cos(check_angle);
|
||||
double yA1 = pose_C.pose.position.y + rCA*sin(check_angle);
|
||||
double xCA1 = xA1 - pose_C.pose.position.x;
|
||||
double yCA1 = yA1 - pose_C.pose.position.y;
|
||||
double cos_A1CB = (xCA1*xCB + yCA1*yCB)/(rCA*rCB);
|
||||
if(cos_A1CB>1) cos_A1CB = 1;
|
||||
else if(cos_A1CB<(-1)) cos_A1CB = -1;
|
||||
double angleA1CB = acos(cos_A1CB);
|
||||
if(angleA1CB>angleACB)
|
||||
{
|
||||
is_increase_angle = false;
|
||||
}
|
||||
else if(angleA1CB<angleACB)
|
||||
{
|
||||
is_increase_angle = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Curve AB is too short, cannot compute plan");
|
||||
return false;
|
||||
}
|
||||
if(is_increase_angle)
|
||||
{
|
||||
for(double i = 0; i<=1; i+= angle_interval)
|
||||
{
|
||||
double angle_tmp = angleCA + angleACB*i;
|
||||
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
|
||||
double yP = pose_C.pose.position.y + rCA*sin(angle_tmp);
|
||||
geometry_msgs::PoseStamped p;
|
||||
p.pose.position.x = xP;
|
||||
p.pose.position.y = yP;
|
||||
p.pose.position.z = 0;
|
||||
PlanRetry_2.push_back(p);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for(double i = 0; i<=1; i+= angle_interval)
|
||||
{
|
||||
double angle_tmp = angleCA - angleACB*i;
|
||||
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
|
||||
double yP = pose_C.pose.position.y + rCA*sin(angle_tmp);
|
||||
geometry_msgs::PoseStamped p;
|
||||
p.pose.position.x = xP;
|
||||
p.pose.position.y = yP;
|
||||
p.pose.position.z = 0;
|
||||
PlanRetry_2.push_back(p);
|
||||
}
|
||||
}
|
||||
if(!PlanRetry_2.empty()&&PlanRetry_2.size()>2)
|
||||
{
|
||||
for(int i = 0 ; i < PlanRetry_2.size(); i++)
|
||||
{
|
||||
ROS_INFO("Pose %d in PlanRetry : %f, %f", i, PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y);
|
||||
}
|
||||
if(/*computeDeltaAngleStartOfPlan(getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w),
|
||||
PlanRetry_2.front().pose, PlanRetry_2[1].pose) <= 1.3962634016 &&*/
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
PlanRetry_2.back().pose, PlanRetry_2[PlanRetry_2.size() - 2].pose) <= 1.3962634016
|
||||
) // <= 80 degree
|
||||
{
|
||||
double theta_tmp = 0;
|
||||
for(int i = 0; i<((int)PlanRetry_2.size()-1); i++)
|
||||
{
|
||||
if(PlanRetry_2[i].pose.position.x!=PlanRetry_2[i+1].pose.position.x)
|
||||
{
|
||||
double theta = calculateAngle(PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y,
|
||||
PlanRetry_2[i+1].pose.position.x, PlanRetry_2[i+1].pose.position.y);
|
||||
PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta);
|
||||
theta_tmp = theta;
|
||||
}
|
||||
else
|
||||
{
|
||||
PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp);
|
||||
}
|
||||
}
|
||||
PlanRetry_2.back().pose.orientation = pose_B.pose.orientation;
|
||||
}
|
||||
else if(/*computeDeltaAngleStartOfPlan(getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w),
|
||||
PlanRetry_2.front().pose, PlanRetry_2[1].pose) >= 1.745329252 &&*/
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
PlanRetry_2.back().pose, PlanRetry_2[PlanRetry_2.size() - 2].pose) >= 1.745329252) // >= 100 degree
|
||||
{
|
||||
double theta_tmp = 0;
|
||||
for(int i = (int)PlanRetry_2.size() -1; i>0; i--)
|
||||
{
|
||||
if(PlanRetry_2[i].pose.position.x!=PlanRetry_2[i-1].pose.position.x)
|
||||
{
|
||||
double theta = calculateAngle(PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y,
|
||||
PlanRetry_2[i-1].pose.position.x, PlanRetry_2[i-1].pose.position.y);
|
||||
PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta);
|
||||
theta_tmp = theta;
|
||||
}
|
||||
else
|
||||
{
|
||||
PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp);
|
||||
}
|
||||
}
|
||||
PlanRetry_2.front().pose.orientation = PlanRetry_2[1].pose.orientation;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Pose_A yaw or Pose_B yaw is invalid value");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Curve AB is too short, cannot compute plan");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Tạo đoạn đường từ poseB -> poseB_behind
|
||||
PlanRetry_3 = divideSegment(pose_B, pose_B_behind, 0.1);
|
||||
|
||||
ros::Time plan_time = ros::Time::now();
|
||||
if(!PlanRetry_1.empty()&&!PlanRetry_2.empty()&&!PlanRetry_3.empty())
|
||||
{
|
||||
for(int i = 0; i < (int)PlanRetry_1.size(); i++)
|
||||
{
|
||||
PlanRetry_1[i].header.stamp = plan_time;
|
||||
result_plan.push_back(PlanRetry_1[i]);
|
||||
}
|
||||
for(int i = 0; i < (int)PlanRetry_2.size(); i++)
|
||||
{
|
||||
PlanRetry_2[i].header.stamp = plan_time;
|
||||
PlanRetry_2[i].header.frame_id = PlanRetry_1.front().header.frame_id;
|
||||
result_plan.push_back(PlanRetry_2[i]);
|
||||
}
|
||||
for(int i = 0; i < (int)PlanRetry_3.size(); i++)
|
||||
{
|
||||
PlanRetry_3[i].header.stamp = plan_time;
|
||||
PlanRetry_3[i].header.frame_id = PlanRetry_1.front().header.frame_id;
|
||||
result_plan.push_back(PlanRetry_3[i]);
|
||||
}
|
||||
result = true;
|
||||
}
|
||||
else{
|
||||
if(PlanRetry_3.empty())
|
||||
{
|
||||
ROS_WARN("PlanRetry_3 is empty");
|
||||
}
|
||||
if(PlanRetry_2.empty())
|
||||
{
|
||||
ROS_WARN("PlanRetry_2 is empty");
|
||||
}
|
||||
if(PlanRetry_1.empty())
|
||||
{
|
||||
ROS_WARN("PlanRetry_1 is empty");
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// Hàm gọi make plan : tạo tuyến đường đi quay lại điểm retry có trong plan hiện tại và đi tới điểm goal mới theo cung tròn.
|
||||
// khi tạo thành công plan thì hàm trả về True, không thành công thì trả về False và có hiện cảnh báo nguyên nhân.
|
||||
// current_plan: vector chứa plan hiện tại
|
||||
// pose_A: điểm quay lại để retry
|
||||
// pose_B: điểm goal mới, có hướng thẳng với palet hiện tại
|
||||
// pose_C: tâm của cung tròn AB
|
||||
// result_plan: vector chứa plan kết quả
|
||||
|
||||
bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_B_behind,
|
||||
geometry_msgs::PoseStamped& pose_C, std::vector<geometry_msgs::PoseStamped>& result_plan)
|
||||
{
|
||||
bool result = false;
|
||||
std::vector<geometry_msgs::PoseStamped> PlanRetry_1;
|
||||
std::vector<geometry_msgs::PoseStamped> PlanRetry_2;
|
||||
std::vector<geometry_msgs::PoseStamped> PlanRetry_3;
|
||||
|
||||
ROS_INFO("[makePlanForRetry] pose_A: %f, %f, %f pose_B: %f, %f, %f pose_C: %f, %f",
|
||||
pose_A.pose.position.x, pose_A.pose.position.y,
|
||||
getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w),
|
||||
pose_B.pose.position.x, pose_B.pose.position.y,
|
||||
getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w));
|
||||
|
||||
// Tính ra PlanRetry_1 điểm retry tại Pose_A
|
||||
|
||||
PlanRetry_1 = divideSegment(pose_B, pose_A, 0.1);
|
||||
|
||||
// Tính ra PlanRetry_2 với biên dạng cung tròn đi qua pose_A và pose_B, có tâm tại pose_C
|
||||
|
||||
double xAB = pose_B.pose.position.x - pose_A.pose.position.x;
|
||||
double yAB = pose_B.pose.position.y - pose_A.pose.position.y;
|
||||
double d_AB = sqrt(xAB*xAB + yAB*yAB);
|
||||
if(d_AB<=0.1)
|
||||
{
|
||||
ROS_WARN("Curve AB is too short, cannot compute plan");
|
||||
return false;
|
||||
}
|
||||
|
||||
// nếu hướng của vector AB và hướng của pose_B tạo với nhau một góc ~0 độ hoặc ~180 độ -> cung tròn AB sẽ gần như là một đọan thẳng
|
||||
if((computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) >= 3.13 &&
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) <= M_PI) ||
|
||||
(computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) <= 0.1745 &&
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) >= 0))
|
||||
{
|
||||
std::vector<geometry_msgs::PoseStamped> planSegment_AB;
|
||||
planSegment_AB = divideSegment(pose_A, pose_B, 0.1);
|
||||
PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end());
|
||||
}
|
||||
else
|
||||
{
|
||||
double xCA = pose_A.pose.position.x - pose_C.pose.position.x;
|
||||
double yCA = pose_A.pose.position.y - pose_C.pose.position.y;
|
||||
double xCB = pose_B.pose.position.x - pose_C.pose.position.x;
|
||||
double yCB = pose_B.pose.position.y - pose_C.pose.position.y;
|
||||
double rCA = sqrt(xCA*xCA + yCA*yCA);
|
||||
double rCB = sqrt(xCB*xCB + yCB*yCB);
|
||||
if(abs(rCA-rCB)>0.008)
|
||||
{
|
||||
ROS_WARN("pose_C is not Center of Curve AB");
|
||||
return false;
|
||||
}
|
||||
|
||||
double cos_ACB = (xCA*xCB + yCA*yCB)/(rCA*rCB);
|
||||
if(cos_ACB>1) cos_ACB = 1;
|
||||
else if(cos_ACB<(-1)) cos_ACB = -1;
|
||||
double angleACB = acos(cos_ACB);
|
||||
double angle_interval = 0.005;
|
||||
// tính góc của vector CA:
|
||||
double angleCA = atan2(yCA, xCA);
|
||||
|
||||
// check thử xem chiều góc quét từ A -> B thì angleCA + delta_angle hay angleCA - delta_angle
|
||||
bool is_increase_angle = false;
|
||||
double check_angle = angleCA + 50*angle_interval*angleACB;
|
||||
double xA1 = pose_C.pose.position.x + rCA*cos(check_angle);
|
||||
double yA1 = pose_C.pose.position.y + rCA*sin(check_angle);
|
||||
double xCA1 = xA1 - pose_C.pose.position.x;
|
||||
double yCA1 = yA1 - pose_C.pose.position.y;
|
||||
double cos_A1CB = (xCA1*xCB + yCA1*yCB)/(rCA*rCB);
|
||||
if(cos_A1CB>1) cos_A1CB = 1;
|
||||
else if(cos_A1CB<(-1)) cos_A1CB = -1;
|
||||
double angleA1CB = acos(cos_A1CB);
|
||||
if(angleA1CB>angleACB)
|
||||
{
|
||||
is_increase_angle = false;
|
||||
}
|
||||
else if(angleA1CB<angleACB)
|
||||
{
|
||||
is_increase_angle = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Curve AB is too short, cannot compute plan");
|
||||
return false;
|
||||
}
|
||||
if(is_increase_angle)
|
||||
{
|
||||
for(double i = 0; i<=1; i+= angle_interval)
|
||||
{
|
||||
double angle_tmp = angleCA + angleACB*i;
|
||||
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
|
||||
double yP = pose_C.pose.position.y + rCA*sin(angle_tmp);
|
||||
geometry_msgs::PoseStamped p;
|
||||
p.pose.position.x = xP;
|
||||
p.pose.position.y = yP;
|
||||
p.pose.position.z = 0;
|
||||
PlanRetry_2.push_back(p);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for(double i = 0; i<=1; i+= angle_interval)
|
||||
{
|
||||
double angle_tmp = angleCA - angleACB*i;
|
||||
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
|
||||
double yP = pose_C.pose.position.y + rCA*sin(angle_tmp);
|
||||
geometry_msgs::PoseStamped p;
|
||||
p.pose.position.x = xP;
|
||||
p.pose.position.y = yP;
|
||||
p.pose.position.z = 0;
|
||||
PlanRetry_2.push_back(p);
|
||||
}
|
||||
}
|
||||
if(!PlanRetry_2.empty()&&PlanRetry_2.size()>2)
|
||||
{
|
||||
for(int i = 0 ; i < PlanRetry_2.size(); i++)
|
||||
{
|
||||
ROS_INFO("Pose %d in PlanRetry : %f, %f", i, PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y);
|
||||
}
|
||||
if(/*computeDeltaAngleStartOfPlan(getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w),
|
||||
PlanRetry_2.front().pose, PlanRetry_2[1].pose) <= 1.3962634016 &&*/
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
PlanRetry_2.back().pose, PlanRetry_2[PlanRetry_2.size() - 2].pose) <= 1.3962634016
|
||||
) // <= 80 degree
|
||||
{
|
||||
double theta_tmp = 0;
|
||||
for(int i = 0; i<((int)PlanRetry_2.size()-1); i++)
|
||||
{
|
||||
if(PlanRetry_2[i].pose.position.x!=PlanRetry_2[i+1].pose.position.x)
|
||||
{
|
||||
double theta = calculateAngle(PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y,
|
||||
PlanRetry_2[i+1].pose.position.x, PlanRetry_2[i+1].pose.position.y);
|
||||
PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta);
|
||||
theta_tmp = theta;
|
||||
}
|
||||
else
|
||||
{
|
||||
PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp);
|
||||
}
|
||||
}
|
||||
PlanRetry_2.back().pose.orientation = pose_B.pose.orientation;
|
||||
}
|
||||
else if(/*computeDeltaAngleStartOfPlan(getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w),
|
||||
PlanRetry_2.front().pose, PlanRetry_2[1].pose) >= 1.745329252 &&*/
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
PlanRetry_2.back().pose, PlanRetry_2[PlanRetry_2.size() - 2].pose) >= 1.745329252) // >= 100 degree
|
||||
{
|
||||
double theta_tmp = 0;
|
||||
for(int i = (int)PlanRetry_2.size() -1; i>0; i--)
|
||||
{
|
||||
if(PlanRetry_2[i].pose.position.x!=PlanRetry_2[i-1].pose.position.x)
|
||||
{
|
||||
double theta = calculateAngle(PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y,
|
||||
PlanRetry_2[i-1].pose.position.x, PlanRetry_2[i-1].pose.position.y);
|
||||
PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta);
|
||||
theta_tmp = theta;
|
||||
}
|
||||
else
|
||||
{
|
||||
PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp);
|
||||
}
|
||||
}
|
||||
PlanRetry_2.front().pose.orientation = PlanRetry_2[1].pose.orientation;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Pose_A yaw or Pose_B yaw is invalid value");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Curve AB is too short, cannot compute plan");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Tạo đoạn đường từ poseB -> poseB_behind
|
||||
PlanRetry_3 = divideSegment(pose_B, pose_B_behind, 0.1);
|
||||
|
||||
ros::Time plan_time = ros::Time::now();
|
||||
if(!PlanRetry_1.empty()&&!PlanRetry_2.empty()&&!PlanRetry_3.empty())
|
||||
{
|
||||
for(int i = 0; i < (int)PlanRetry_1.size(); i++)
|
||||
{
|
||||
PlanRetry_1[i].header.stamp = plan_time;
|
||||
result_plan.push_back(PlanRetry_1[i]);
|
||||
}
|
||||
for(int i = 0; i < (int)PlanRetry_2.size(); i++)
|
||||
{
|
||||
PlanRetry_2[i].header.stamp = plan_time;
|
||||
PlanRetry_2[i].header.frame_id = PlanRetry_1.front().header.frame_id;
|
||||
result_plan.push_back(PlanRetry_2[i]);
|
||||
}
|
||||
for(int i = 0; i < (int)PlanRetry_3.size(); i++)
|
||||
{
|
||||
PlanRetry_3[i].header.stamp = plan_time;
|
||||
PlanRetry_3[i].header.frame_id = PlanRetry_1.front().header.frame_id;
|
||||
result_plan.push_back(PlanRetry_3[i]);
|
||||
}
|
||||
result = true;
|
||||
}
|
||||
else{
|
||||
if(PlanRetry_3.empty())
|
||||
{
|
||||
ROS_WARN("PlanRetry_3 is empty");
|
||||
}
|
||||
if(PlanRetry_2.empty())
|
||||
{
|
||||
ROS_WARN("PlanRetry_2 is empty");
|
||||
}
|
||||
if(PlanRetry_1.empty())
|
||||
{
|
||||
ROS_WARN("PlanRetry_1 is empty");
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// Hàm Tìm tâm C của cung tròn AB khi biết Pose tại điểm B: khi tìm thành công điểm C thì hàm trả về True
|
||||
// pose_A: điểm start của cung tròn
|
||||
// pose_B: điểm đích trên cung tròn
|
||||
// pose_C: tâm của cung tròn AB (kết quả)
|
||||
|
||||
bool findCenterOfCurve(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_C)
|
||||
{
|
||||
// nếu hướng của vector AB và hướng của pose_B tạo với nhau một góc ~0 độ hoặc ~180 độ -> điểm C sẽ gần xấp xỉ với trung điểm của đoạn thẳng AB.
|
||||
if((computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) >= 3.13 &&
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) <= M_PI) ||
|
||||
(computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) <= 0.1745 &&
|
||||
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
|
||||
pose_B.pose, pose_A.pose) >= 0))
|
||||
{
|
||||
pose_C.pose.position.x = (pose_A.pose.position.x + pose_B.pose.position.x)/2;
|
||||
pose_C.pose.position.y = (pose_A.pose.position.y + pose_B.pose.position.y)/2;
|
||||
}
|
||||
else
|
||||
{
|
||||
double x_R = pose_A.pose.position.x;
|
||||
double y_R = pose_A.pose.position.y;
|
||||
double x_G = pose_B.pose.position.x;
|
||||
double y_G = pose_B.pose.position.y;
|
||||
double phi_vG = getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w);
|
||||
double x_H = (x_R+x_G)/2;
|
||||
double y_H = (y_R+y_G)/2;
|
||||
double m_vG = tan(phi_vG);
|
||||
double m_G_n_vG = -1/m_vG;
|
||||
double b_G_n_vG = y_G-m_G_n_vG*x_G;
|
||||
double m_RG =(y_G-y_R)/(x_G-x_R);
|
||||
double b_RG = y_R-m_RG*x_R;
|
||||
double m_H_n_RG = -1/m_RG;
|
||||
double b_H_n_RG = y_H-m_H_n_RG*x_H;
|
||||
pose_C.pose.position.x = (b_H_n_RG-b_G_n_vG)/(m_G_n_vG-m_H_n_RG);
|
||||
pose_C.pose.position.y = (b_H_n_RG*m_G_n_vG-b_G_n_vG*m_H_n_RG)/(m_G_n_vG-m_H_n_RG);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
42
src/pose.cc
Executable file
42
src/pose.cc
Executable file
|
|
@ -0,0 +1,42 @@
|
|||
#include <custom_planner/pose.h>
|
||||
|
||||
namespace custom_planner
|
||||
{
|
||||
|
||||
bool Pose::SavePoseAsFile(const string& file_name)
|
||||
{
|
||||
bool result = false;
|
||||
ofstream file(file_name);
|
||||
if (file.is_open())
|
||||
{
|
||||
file << x_ << " " << y_ << " "<< yaw_ << "\n";
|
||||
file.close();
|
||||
result = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
result = false;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool Pose::LoadPoseFromFile(const string& file_name)
|
||||
{
|
||||
bool result = false;
|
||||
ifstream file(file_name);
|
||||
if (file.is_open())
|
||||
{
|
||||
double x, y, yaw;
|
||||
file >> x >> y >> yaw;
|
||||
setPose(x, y, yaw);
|
||||
file.close();
|
||||
result = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
result = false;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user