DuongTD/custom_planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: DuongTD:377ebe3d6f66c6a1c7f2d1a1d76fd1b330a93952
Branches Tags
DuongTD:main
..
compare: DuongTD:main
Branches Tags
DuongTD:main

9 Commits
377ebe3d6f ... main

Author SHA1 Message Date
HiepLM
43810ce140 update tf3 2026-02-07 10:57:37 +07:00
duongtd
e0f6738c31 optimal 2026-01-30 10:56:41 +07:00
HiepLM
49afcce5b2 update 2026-01-16 15:12:17 +07:00
HiepLM
a2bebb5be9 CustomPlanner 2026-01-09 10:38:55 +07:00
HiepLM
540d79321b update for ROS 2026-01-07 16:54:53 +07:00
HiepLM
0b01c22019 robot_nav_core 2026-01-07 09:19:10 +07:00
HiepLM
60e9c5673f HiepLM update 2025-12-30 10:22:16 +07:00
HiepLM
d6512018ef hiep sua ten file 2025-12-30 09:57:13 +07:00
HiepLM
6d55c6c4be Hiep update 2025-12-30 09:06:20 +07:00
13 changed files with 542 additions and 475 deletions
Expand all files Collapse all files

244
CMakeLists.txt
View File

@@ -1,39 +1,89 @@
# --- CMake version và project name ---
cmake_minimum_required(VERSION 3.10)
project(custom_planner)
cmake_minimum_required(VERSION 3.0.2)
project(custom_planner VERSION 1.0.0 LANGUAGES CXX)
# --- 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
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building custom_planner with Catkin")
# --- 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)
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building custom_planner with Standalone CMake")
endif()
# --- 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
# C++ Standard - must be set before find_package
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# --- Include directories ---
# Thêm các folder chứa header files
include_directories(
# Find dependencies
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system thread filesystem)
if (NOT BUILDING_WITH_CATKIN)
# Enable Position Independent Code
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# Cấu hình RPATH để tránh cycle trong runtime search path
set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
set(PACKAGES_DIR
robot_visualization_msgs
robot_nav_msgs
robot_std_msgs
robot_geometry_msgs
tf3
robot_tf3_geometry_msgs
robot_time
data_convert
robot_costmap_2d
robot_nav_core
robot_protocol_msgs
robot_cpp
)
else()
# ========================================================
# Catkin specific configuration
# ========================================================
find_package(catkin REQUIRED COMPONENTS
robot_visualization_msgs
robot_nav_msgs
robot_std_msgs
robot_geometry_msgs
robot_tf3_geometry_msgs
robot_time
data_convert
robot_costmap_2d
robot_nav_core
robot_protocol_msgs
robot_cpp
)
find_library(TF3_LIBRARY NAMES tf3)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS robot_visualization_msgs robot_nav_msgs robot_std_msgs robot_geometry_msgs robot_tf3_geometry_msgs robot_time data_convert robot_costmap_2d robot_nav_core robot_protocol_msgs robot_cpp
DEPENDS Eigen3 Boost
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
)
endif()
# --- Eigen và PCL definitions ---
add_definitions(${EIGEN3_DEFINITIONS})
# --- Core library: custom_planner ---
# Tạo thư viện chính
add_library(custom_planner
# ========================================================
# Libraries
# ========================================================
add_library(${PROJECT_NAME} SHARED
src/custom_planner.cpp
src/pathway.cc
src/pose.cc
@@ -41,70 +91,90 @@ add_library(custom_planner
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
)
if(BUILDING_WITH_CATKIN)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# --- Include directories cho target ---
target_include_directories(custom_planner
target_include_directories(${PROJECT_NAME}
PUBLIC
${Boost_INCLUDE_DIRS} # Boost headers
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> # Khi build từ source
$<INSTALL_INTERFACE:include/${PROJECT_NAME}> # Khi install
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# --- 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
)
target_link_libraries(${PROJECT_NAME}
PUBLIC ${catkin_LIBRARIES}
PRIVATE Eigen3::Eigen Boost::system Boost::thread Boost::filesystem
)
# --- 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
)
else()
# --- Cài đặt headers ---
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
)
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# # --- 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(${PROJECT_NAME}
PUBLIC
${PACKAGES_DIR}
PRIVATE
Eigen3::Eigen Boost::system Boost::thread Boost::filesystem
)
# target_link_libraries(plugins
# custom_planner
# ${Boost_LIBRARIES}
# yaml-cpp
# robot_time
# )
set_target_properties(${PROJECT_NAME} PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
)
endif()
# ========================================================
# Install
# ========================================================
if(BUILDING_WITH_CATKIN)
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
else()
install(TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}-targets
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
# Export targets
install(EXPORT ${PROJECT_NAME}-targets
FILE ${PROJECT_NAME}-targets.cmake
NAMESPACE ${PROJECT_NAME}::
DESTINATION lib/cmake/${PROJECT_NAME}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
# Print configuration info
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Dependencies: robot_visualization_msgs, robot_nav_msgs, robot_std_msgs, robot_geometry_msgs, tf3, robot_tf3_geometry_msgs, robot_time, data_convert, robot_costmap_2d, robot_nav_core, robot_protocol_msgs, robot_cpp, Eigen3, Boost")
message(STATUS "=================================")
endif()

34
include/custom_planner/Curve_common.h
View File

@@ -7,9 +7,9 @@
// #include <deque>
// #include <iostream>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include <robot_nav_msgs/Path.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_visualization_msgs/Marker.h>
#include "custom_planner/color.h"
struct Spline_Inf
@@ -47,16 +47,16 @@ 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);
robot_nav_msgs::Path Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
robot_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);
robot_geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS);
robot_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);
@@ -67,15 +67,15 @@ class Curve_common
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);
void ShowDiscreatePoint(robot_visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point);
void ShowDiscreatePoint(robot_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);
robot_visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
robot_visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale);
robot_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);
robot_visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale);
private:

6
include/custom_planner/color.h
View File

@@ -1,14 +1,14 @@
#ifndef CUSTOM_PLANNER_COLOR_H_
#define CUSTOM_PLANNER_COLOR_H_
#include <std_msgs/ColorRGBA.h>
#include <robot_std_msgs/ColorRGBA.h>
namespace agv_visualization
{
class Color : public std_msgs::ColorRGBA
class Color : public robot_std_msgs::ColorRGBA
{
public:
Color() : std_msgs::ColorRGBA() {}
Color() : robot_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;

10
include/custom_planner/conversion.h
View File

@@ -3,14 +3,14 @@
#include "custom_planner/Curve_common.h"
#include <Eigen/Geometry>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Quaternion.h>
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) {
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const robot_geometry_msgs::Point& msg) {
return Eigen::Vector3d(msg.x, msg.y, msg.z);
}
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::PoseStamped> &plan)
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::PoseStamped> &plan)
{
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point;
@@ -25,7 +25,7 @@ inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::v
return eigen_trajectory_point_vec;
}
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::Point> &discreate_point_vec)
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::Point> &discreate_point_vec)
{
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point;

123
include/custom_planner/custom_planner.h
View File

@@ -9,14 +9,14 @@
using namespace std;
// ROS
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_visualization_msgs/Marker.h>
// Costmap used for the map representation
#include <costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
// global representation
#include <nav_core/base_global_planner.h>
#include <robot_nav_core/base_global_planner.h>
// tf
#include <tf3/convert.h>
@@ -29,18 +29,8 @@ using namespace std;
#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_geometry_msgs/PoseArray.h>
#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;
@@ -56,7 +46,7 @@ struct OrderNode{
double theta;
};
class CustomPlanner : public nav_core::BaseGlobalPlanner{
class CustomPlanner : public robot_nav_core::BaseGlobalPlanner{
public:
/**
@@ -70,7 +60,7 @@ public:
* @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);
CustomPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
/**
@@ -79,12 +69,12 @@ public:
* @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);
robot_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);
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan);
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
@@ -92,9 +82,9 @@ public:
* @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)
virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
printf("[%s:%d] This function is not available!",__FILE__,__LINE__);
return false;
@@ -104,33 +94,24 @@ public:
private:
void publishStats(int solution_cost, int solution_size,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal);
const robot_geometry_msgs::PoseStamped& start,
const robot_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);
static void transformFootprintToEdges(const robot_geometry_msgs::Pose& robot_pose,
const std::vector<robot_geometry_msgs::Point>& footprint,
std::vector<robot_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);
const robot_geometry_msgs::PoseStamped &goal);
bool loadPathwayData(const string& filename);
@@ -139,17 +120,17 @@ private:
// 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);
const robot_geometry_msgs::PoseStamped& start,
const robot_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);
bool doIntersect(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_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);
std::optional<robot_geometry_msgs::Point> computeIntersectionPoint(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2);
bool calcAllYaw(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
bool calcAllYaw(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
vector<Pose>& saveposesOnEdge,
int& total_edge);
@@ -163,7 +144,7 @@ private:
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<robot_geometry_msgs::PoseStamped> divideSegment(robot_geometry_msgs::PoseStamped& A, robot_geometry_msgs::PoseStamped& B, double d);
// Vector lưu thông tin các cạnh
std::vector<InfEdge> edges_info_;
@@ -173,55 +154,19 @@ private:
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;
Spline_Inf* input_spline_inf_;
Curve_common* CurveDesign_;
vector<Pose> posesOnPathWay_;
// 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_;
robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
std::vector<robot_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_;
};
}

12
include/custom_planner/custom_planner_config.h Normal file
View File

@@ -0,0 +1,12 @@
#ifndef CUSTOM_PLANNER_CONFIG_H_
#define CUSTOM_PLANNER_CONFIG_H_
#include <robot/robot.h>
namespace custom_planner
{
// struct
}
#endif // CUSTOM_PLANNER_CONFIG_H_

16
include/custom_planner/merge_path_calc.h
View File

@@ -1,6 +1,6 @@
#ifndef MERGER_PATH_CALC_H_
#define MERGER_PATH_CALC_H_
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <vector>
#include <cmath>
@@ -96,8 +96,8 @@ namespace custom_planner
* 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,
bool findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
const robot_geometry_msgs::PoseStamped& start,
std::vector<Pose>& posesOnPathWay,
point_type type);
@@ -109,7 +109,7 @@ namespace custom_planner
* 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,
std::vector<robot_geometry_msgs::PoseStamped> calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points,
bool reverse);
bool handleEdgeIntersection(vector<Pose>& savePosesOnEdge,
@@ -174,7 +174,7 @@ namespace custom_planner
private:
// vector<geometry_msgs::PoseStamped> control_points;
// vector<robot_geometry_msgs::PoseStamped> control_points;
bool normal_plag = false;
@@ -191,8 +191,8 @@ namespace custom_planner
* 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,
void updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
bool reverse);
/**
@@ -204,7 +204,7 @@ namespace custom_planner
* 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,
static void setYawAllPosesOnEdge(std::vector<robot_geometry_msgs::PoseStamped>& Poses,
bool reverse = false);

55
package.xml Normal file
View File

@@ -0,0 +1,55 @@
<package>
<name>custom_planner</name>
<version>0.7.10</version>
<description>
custom_planner is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. custom_planner
maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired
point in time.
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>
<author>Wim Meeussen</author>
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/custom_planner</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>robot_costmap_2d</build_depend>
<run_depend>robot_costmap_2d</run_depend>
<build_depend>robot_nav_core</build_depend>
<run_depend>robot_nav_core</run_depend>
<build_depend>robot_geometry_msgs</build_depend>
<run_depend>robot_geometry_msgs</run_depend>
<build_depend>robot_nav_msgs</build_depend>
<run_depend>robot_nav_msgs</run_depend>
<build_depend>robot_std_msgs</build_depend>
<run_depend>robot_std_msgs</run_depend>
<build_depend>robot_tf3_geometry_msgs</build_depend>
<run_depend>robot_tf3_geometry_msgs</run_depend>
<build_depend>robot_cpp</build_depend>
<run_depend>robot_cpp</run_depend>
<build_depend>robot_protocol_msgs</build_depend>
<run_depend>robot_protocol_msgs</run_depend>
<build_depend>robot_time</build_depend>
<run_depend>robot_time</run_depend>
<build_depend>robot_visualization_msgs</build_depend>
<run_depend>robot_visualization_msgs</run_depend>
<build_depend>data_convert</build_depend>
<run_depend>data_convert</run_depend>
</package>

90
src/Curve_common.cpp
View File

@@ -1,7 +1,7 @@
#include "custom_planner/Curve_common.h"
#include "custom_planner/conversion.h"
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <iostream>
@@ -10,10 +10,10 @@ Curve_common::Curve_common()
}
nav_msgs::Path Curve_common::Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id)
{
nav_msgs::Path line_result;
geometry_msgs::PoseStamped current_pose;
robot_nav_msgs::Path line_result;
robot_geometry_msgs::PoseStamped current_pose;
line_result.header.frame_id = frame_id;
line_result.header.stamp = robot::Time::now();
@@ -38,10 +38,10 @@ nav_msgs::Path Curve_common::Generate_Line(geometry_msgs::Point start_point, geo
return line_result;
}
nav_msgs::Path Curve_common::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id)
{
nav_msgs::Path bezier_curve_result;
geometry_msgs::PoseStamped current_pose;
robot_nav_msgs::Path bezier_curve_result;
robot_geometry_msgs::PoseStamped current_pose;
EigenTrajectoryPoint::Vector temp_control_point_vec;
bezier_curve_result.header.frame_id = frame_id;
@@ -136,13 +136,13 @@ void Curve_common::ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, E
// }
}
visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale)
robot_visualization_msgs::Marker Curve_common::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 waypoints_marker;
robot_visualization_msgs::Marker waypoints_marker;
waypoints_marker.header.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now();
waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST;
waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
//waypoints_marker.color = color;
waypoints_marker.color.r = 1;
waypoints_marker.color.g = 1;
@@ -154,7 +154,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::vector<Eigen::V
waypoints_marker.points.reserve(static_cast<int>(discreate_point.size()));
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i)(0);
@@ -165,13 +165,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::vector<Eigen::V
return waypoints_marker;
}
visualization_msgs::Marker Curve_common::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)
robot_visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale)
{
visualization_msgs::Marker waypoints_marker;
robot_visualization_msgs::Marker waypoints_marker;
waypoints_marker.header.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now();
waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST;
waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
waypoints_marker.color = point_color;
waypoints_marker.color.a = 0.75;
waypoints_marker.ns = name;
@@ -181,7 +181,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector<Eigen::
waypoints_marker.points.reserve(static_cast<int>(discreate_point.size()));
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i)(0);
@@ -192,13 +192,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector<Eigen::
return waypoints_marker;
}
visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale)
robot_visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale)
{
visualization_msgs::Marker waypoints_marker;
robot_visualization_msgs::Marker waypoints_marker;
waypoints_marker.header.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now();
waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST;
waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
//waypoints_marker.color = color;
waypoints_marker.color.r = 1;
waypoints_marker.color.g = 1;
@@ -210,7 +210,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint
waypoints_marker.points.reserve(static_cast<int>(discreate_point.size()));
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i).position(0);
@@ -221,13 +221,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint
return waypoints_marker;
}
visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale)
robot_visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale)
{
visualization_msgs::Marker waypoints_marker;
robot_visualization_msgs::Marker waypoints_marker;
waypoints_marker.header.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now();
waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST;
waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
waypoints_marker.color = point_color;
// waypoints_marker.color.r = r;
// waypoints_marker.color.g = g;
@@ -240,7 +240,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoin
waypoints_marker.points.reserve(static_cast<int>(discreate_point.size()));
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i).position(0);
@@ -251,9 +251,9 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoin
return waypoints_marker;
}
void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point)
void Curve_common::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point)
{
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i).position(0);
@@ -262,9 +262,9 @@ void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenT
}
}
void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point)
void Curve_common::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point)
{
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i)(0);
@@ -326,10 +326,10 @@ void Curve_common::ReadSplineInf(Spline_Inf *spline_inf, std::vector<double> wei
// }
}
nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id)
{
nav_msgs::Path bspline_curve_result;
geometry_msgs::PoseStamped current_pose;
robot_nav_msgs::Path bspline_curve_result;
robot_geometry_msgs::PoseStamped current_pose;
bspline_curve_result.header.frame_id = frame_id;
bspline_curve_result.header.stamp = robot::Time::now();
@@ -500,10 +500,10 @@ nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, doubl
return bspline_curve_result;
}
nav_msgs::Path Curve_common::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id)
{
nav_msgs::Path nurbs_curve_result;
geometry_msgs::PoseStamped current_pose;
robot_nav_msgs::Path nurbs_curve_result;
robot_geometry_msgs::PoseStamped current_pose;
nurbs_curve_result.header.frame_id = frame_id;
nurbs_curve_result.header.stamp = robot::Time::now();
@@ -754,9 +754,9 @@ void Curve_common::CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u
}
}
geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS)
robot_geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS)
{
geometry_msgs::Point derivative_curve_point;
robot_geometry_msgs::Point derivative_curve_point;
int p_degree = spline_inf->order - 1;
double sum_x = 0, sum_y = 0;
double sum_denom = 0;
@@ -878,11 +878,11 @@ geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spl
return derivative_curve_point;
}
nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id)
{
geometry_msgs::Point derivative_point_result;
nav_msgs::Path bspline_derivative_result;
geometry_msgs::PoseStamped current_pose;
robot_geometry_msgs::Point derivative_point_result;
robot_nav_msgs::Path bspline_derivative_result;
robot_geometry_msgs::PoseStamped current_pose;
bspline_derivative_result.header.frame_id = frame_id;
bspline_derivative_result.header.stamp = robot::Time::now();
@@ -921,11 +921,11 @@ nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_
return bspline_derivative_result;
}
nav_msgs::Path Curve_common::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id)
{
geometry_msgs::Point derivative_point_result;
nav_msgs::Path derivative_basis_result;
geometry_msgs::PoseStamped current_pose;
robot_geometry_msgs::Point derivative_point_result;
robot_nav_msgs::Path derivative_basis_result;
robot_geometry_msgs::PoseStamped current_pose;
derivative_basis_result.header.frame_id = frame_id;
derivative_basis_result.header.stamp = robot::Time::now();
@@ -1156,11 +1156,11 @@ double Curve_common::CalculateCurveLength(Spline_Inf spline_inf, double start_u,
return sum_length;
}
geometry_msgs::Point Curve_common::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS)
robot_geometry_msgs::Point Curve_common::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS)
{
//TODO: Check u = 1 bug, why x=nan and y=nan?
geometry_msgs::Point curve_point;
robot_geometry_msgs::Point curve_point;
int p_degree = spline_inf->order - 1;
int n = static_cast<int>(spline_inf->control_point.size()) - 1;
//TODO: Check knot vector size and sequence is correect

296
src/custom_planner.cpp
View File

@@ -1,20 +1,12 @@
#include <custom_planner/custom_planner.h>
#include <nav_msgs/Path.h>
#include <costmap_2d/inflation_layer.h>
#include <robot_nav_msgs/Path.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <tf3/LinearMath/Quaternion.h>
#include <boost/dll/alias.hpp>
using namespace std;
namespace geometry_msgs
{
bool operator==(const Point &p1, const Point &p2)
{
return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
}
}
namespace custom_planner
{
CustomPlanner::CustomPlanner()
@@ -22,13 +14,13 @@ namespace custom_planner
{
}
CustomPlanner::CustomPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
CustomPlanner::CustomPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
: initialized_(false), costmap_robot_(NULL)
{
initialize(name, costmap_robot);
}
bool CustomPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
bool CustomPlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
{
if (!initialized_)
{
@@ -36,31 +28,8 @@ namespace custom_planner
robot::NodeHandle p_nh("~");
printf("Name is %s", name.c_str());
pathway = new Pathway();
startPose = new Pose();
input_spline_inf = new Spline_Inf();
CurveDesign = new Curve_common();
private_nh.getParam("planner_type", planner_type_, string("ARAPlanner"));
private_nh.getParam("allocated_time", allocated_time_, 10.0);
private_nh.getParam("initial_epsilon", initial_epsilon_, 3.0);
private_nh.getParam("environment_type", environment_type_, string("XYThetaLattice"));
private_nh.getParam("forward_search", forward_search_, bool(false));
p_nh.getParam("primitive_filename", primitive_filename_, string(""));
private_nh.getParam("force_scratch_limit", force_scratch_limit_, 500);
double nominalvel_mpersecs, timetoturn45degsinplace_secs;
private_nh.getParam("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
private_nh.getParam("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
int lethal_obstacle;
private_nh.getParam("lethal_obstacle", lethal_obstacle, 20);
lethal_obstacle_ = (unsigned char)lethal_obstacle;
inscribed_inflated_obstacle_ = lethal_obstacle_ - 1;
private_nh.getParam("publish_footprint_path", publish_footprint_path_, bool(true));
private_nh.getParam("visualizer_skip_poses", visualizer_skip_poses_, 5);
private_nh.getParam("allow_unknown", allow_unknown_, bool(true));
input_spline_inf_ = new Spline_Inf();
CurveDesign_ = new Curve_common();
name_ = name;
costmap_robot_ = costmap_robot;
@@ -69,13 +38,15 @@ namespace custom_planner
initialized_ = true;
printf("[custom_planner] Initialized successfully");
return true;
}
return false;
}
bool CustomPlanner::makePlan(const robot_protocol_msgs::Order& msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
plan.clear();
@@ -99,8 +70,8 @@ namespace custom_planner
}
// ===== STEP 2: VALIDATE PATHWAY =====
if (posesOnPathWay.empty()) {
printf("[custom_planner] posesOnPathWay is empty");
if (posesOnPathWay_.empty()) {
printf("[custom_planner] posesOnPathWay_ is empty");
return false;
}
@@ -114,9 +85,9 @@ namespace custom_planner
// ===== STEP 4: FIND NEAREST POINT ON PATHWAY =====
int nearest_idx = 0;
double min_dist = std::numeric_limits<double>::max();
for (int i = 0; i < static_cast<int>(posesOnPathWay.size()); ++i) {
double distance = std::hypot(posesOnPathWay[i].getX() - start.pose.position.x,
posesOnPathWay[i].getY() - start.pose.position.y);
for (int i = 0; i < static_cast<int>(posesOnPathWay_.size()); ++i) {
double distance = std::hypot(posesOnPathWay_[i].getX() - start.pose.position.x,
posesOnPathWay_[i].getY() - start.pose.position.y);
if (distance < min_dist) {
min_dist = distance;
nearest_idx = i;
@@ -130,13 +101,13 @@ namespace custom_planner
robot::Time plan_time = robot::Time::now();
// Calculate distance from start to nearest pathway point
double dx_start = posesOnPathWay[nearest_idx].getX() - start.pose.position.x;
double dy_start = posesOnPathWay[nearest_idx].getY() - start.pose.position.y;
double dx_start = posesOnPathWay_[nearest_idx].getX() - start.pose.position.x;
double dy_start = posesOnPathWay_[nearest_idx].getY() - start.pose.position.y;
double d_start_to_path = std::sqrt(dx_start * dx_start + dy_start * dy_start);
// Calculate distance from pathway end to goal
double dx_goal = goal.pose.position.x - posesOnPathWay.back().getX();
double dy_goal = goal.pose.position.y - posesOnPathWay.back().getY();
double dx_goal = goal.pose.position.x - posesOnPathWay_.back().getX();
double dy_goal = goal.pose.position.y - posesOnPathWay_.back().getY();
double d_path_to_goal = std::sqrt(dx_goal * dx_goal + dy_goal * dy_goal);
// ===== STEP 6: GENERATE PATH BASED ON DISTANCE CONDITIONS =====
@@ -145,14 +116,14 @@ namespace custom_planner
// CASE 1: Both start and goal are close to pathway - Direct path
printf("[custom_planner] Using direct pathway");
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) {
geometry_msgs::PoseStamped pose;
for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
@@ -160,17 +131,17 @@ namespace custom_planner
// CASE 2: Start is far, goal is close - NURBS from start to pathway
printf("[custom_planner] Using start NURBS path reverse_(%x)", reverse_);
std::vector<geometry_msgs::PoseStamped> start_control_points;
std::vector<robot_geometry_msgs::PoseStamped> start_control_points;
bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints(
start_control_points, start, posesOnPathWay, START);
start_control_points, start, posesOnPathWay_, START);
if (valid_start_nurbs) {
std::vector<geometry_msgs::PoseStamped> nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path =
merge_path_calc_.calculateNURBSPath(start_control_points, reverse_);
// Add NURBS path
for (const auto& pose : nurbs_path) {
geometry_msgs::PoseStamped new_pose = pose;
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -178,26 +149,26 @@ namespace custom_planner
}
// Add remaining pathway
for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay.size(); i++) {
geometry_msgs::PoseStamped pose;
for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
} else {
// Fallback to direct path
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) {
geometry_msgs::PoseStamped pose;
for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
}
@@ -206,29 +177,29 @@ namespace custom_planner
// CASE 3: Start is close, goal is far - NURBS from pathway to goal
printf("[custom_planner] Using goal NURBS path");
std::vector<geometry_msgs::PoseStamped> goal_control_points;
std::vector<robot_geometry_msgs::PoseStamped> goal_control_points;
bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints(
goal_control_points, goal, posesOnPathWay, GOAL);
goal_control_points, goal, posesOnPathWay_, GOAL);
if (valid_goal_nurbs) {
// Add pathway up to goal connection point
for (size_t i = nearest_idx; i < merge_path_calc_.goal_target_idx_; i++) {
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
// Add NURBS path to goal
std::vector<geometry_msgs::PoseStamped> nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path =
merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
for (const auto& pose : nurbs_path) {
geometry_msgs::PoseStamped new_pose = pose;
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -236,14 +207,14 @@ namespace custom_planner
}
} else {
// Fallback: use entire pathway
for (size_t i = 0; i < posesOnPathWay.size(); i++) {
geometry_msgs::PoseStamped pose;
for (size_t i = 0; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
}
@@ -252,11 +223,11 @@ namespace custom_planner
// CASE 4: Both start and goal are far - Dual NURBS or special handling
printf("[custom_planner] Using dual NURBS path");
std::vector<geometry_msgs::PoseStamped> start_control_points, goal_control_points;
std::vector<robot_geometry_msgs::PoseStamped> start_control_points, goal_control_points;
bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints(
start_control_points, start, posesOnPathWay, START);
start_control_points, start, posesOnPathWay_, START);
bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints(
goal_control_points, goal, posesOnPathWay, GOAL);
goal_control_points, goal, posesOnPathWay_, GOAL);
bool valid_index_order = merge_path_calc_.start_target_idx_ < merge_path_calc_.goal_target_idx_;
@@ -265,12 +236,12 @@ namespace custom_planner
costmap_robot_->getRobotPose(goal_control_points[0]);
goal_control_points[goal_control_points.size() - 1] = goal;
std::vector<geometry_msgs::PoseStamped> nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path =
merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
// Skip first point to avoid duplication
for (size_t i = 1; i < nurbs_path.size(); i++) {
geometry_msgs::PoseStamped new_pose = nurbs_path[i];
robot_geometry_msgs::PoseStamped new_pose = nurbs_path[i];
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -281,10 +252,10 @@ namespace custom_planner
// Dual NURBS: Start NURBS + Pathway + Goal NURBS
// Add start NURBS path
std::vector<geometry_msgs::PoseStamped> start_nurbs_path =
merge_path_calc_.calculateNURBSPath(start_control_points, false);
std::vector<robot_geometry_msgs::PoseStamped> start_nurbs_path =
merge_path_calc_.calculateNURBSPath(start_control_points, reverse_);
for (const auto& pose : start_nurbs_path) {
geometry_msgs::PoseStamped new_pose = pose;
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -292,22 +263,22 @@ namespace custom_planner
}
// Add middle pathway segment
for (size_t i = (merge_path_calc_.start_target_idx_ - 1); i < merge_path_calc_.goal_target_idx_; i++) {
geometry_msgs::PoseStamped pose;
for (size_t i = (merge_path_calc_.start_target_idx_ ); i < merge_path_calc_.goal_target_idx_; i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
// Add goal NURBS path
std::vector<geometry_msgs::PoseStamped> goal_nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> goal_nurbs_path =
merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
for (const auto& pose : goal_nurbs_path) {
geometry_msgs::PoseStamped new_pose = pose;
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -317,21 +288,21 @@ namespace custom_planner
} else {
// Fallback: Direct pathway from nearest point
printf("[custom_planner] NURBS control points not found, using fallback path");
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) {
geometry_msgs::PoseStamped pose;
for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY();
pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0;
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw());
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay_[i].getYaw());
plan.push_back(pose);
}
}
}
// ===== STEP 7: ADD FINAL GOAL POSE =====
geometry_msgs::PoseStamped pose_goal;
robot_geometry_msgs::PoseStamped pose_goal;
pose_goal.header.stamp = plan_time;
pose_goal.header.frame_id = costmap_robot_->getGlobalFrameID();
pose_goal.pose = goal.pose;
@@ -344,7 +315,7 @@ namespace custom_planner
}
// Publish plan for visualization
nav_msgs::Path gui_path;
robot_nav_msgs::Path gui_path;
gui_path.header.frame_id = costmap_robot_->getGlobalFrameID();
gui_path.header.stamp = plan_time;
gui_path.poses = plan;
@@ -354,9 +325,9 @@ namespace custom_planner
return true;
}
void CustomPlanner::transformFootprintToEdges(const geometry_msgs::Pose &robot_pose,
const std::vector<geometry_msgs::Point> &footprint,
std::vector<geometry_msgs::Point> &out_footprint)
void CustomPlanner::transformFootprintToEdges(const robot_geometry_msgs::Pose &robot_pose,
const std::vector<robot_geometry_msgs::Point> &footprint,
std::vector<robot_geometry_msgs::Point> &out_footprint)
{
out_footprint.resize(2 * footprint.size());
double yaw = data_convert::getYaw(robot_pose.orientation);
@@ -378,15 +349,15 @@ namespace custom_planner
}
bool CustomPlanner::makePlanWithOrder(robot_protocol_msgs::Order msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal)
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal)
{
vector<Pose> savePosesOnEdge;
int count_two_curve_idx = 0;
//Xóa mảng nếu còn chứa phần tử không xác định
orderNodes.clear();
posesOnPathWay.clear();
std::map<string, OrderNode> orderNodes;
posesOnPathWay_.clear();
edges_info_.clear();
RobotDirectionChangeAngle_info_.clear();
@@ -401,8 +372,8 @@ namespace custom_planner
if(distance_start_to_goal > 0.2) {
return false;
} else {
posesOnPathWay.push_back(Pose(start.pose.position.x, start.pose.position.y, data_convert::getYaw(start.pose.orientation)));
posesOnPathWay.push_back(Pose(goal.pose.position.x, goal.pose.position.y, data_convert::getYaw(goal.pose.orientation)));
posesOnPathWay_.push_back(Pose(start.pose.position.x, start.pose.position.y, data_convert::getYaw(start.pose.orientation)));
posesOnPathWay_.push_back(Pose(goal.pose.position.x, goal.pose.position.y, data_convert::getYaw(goal.pose.orientation)));
return true;
}
}
@@ -460,14 +431,14 @@ namespace custom_planner
{
double t_intervel = 0.01;
order = degree + 1;
input_spline_inf->control_point.clear();
input_spline_inf->knot_vector.clear();
input_spline_inf->weight.clear();
CurveDesign->ReadSplineInf(input_spline_inf, order, control_points, knot_vector);
CurveDesign->ReadSplineInf(input_spline_inf, weight_vector, false);
input_spline_inf_->control_point.clear();
input_spline_inf_->knot_vector.clear();
input_spline_inf_->weight.clear();
CurveDesign_->ReadSplineInf(input_spline_inf_, order, control_points, knot_vector);
CurveDesign_->ReadSplineInf(input_spline_inf_, weight_vector, false);
//Tính độ dài của cạnh thứ i
double edge_length = merge_path_calc_.calculateNURBSLength(CurveDesign, input_spline_inf, t_intervel);
double edge_length = merge_path_calc_.calculateNURBSLength(CurveDesign_, input_spline_inf_, t_intervel);
//Tính step của cạnh thứ i
if (!costmap_robot_ || !costmap_robot_->getCostmap()) {
return false;
@@ -479,8 +450,8 @@ namespace custom_planner
//Tính các điểm theo step mới
for(double u_test = 0; u_test <= 1; u_test += t_intervel_new)
{
geometry_msgs::Point curve_point;
curve_point = CurveDesign->CalculateCurvePoint(input_spline_inf, u_test, true);
robot_geometry_msgs::Point curve_point;
curve_point = CurveDesign_->CalculateCurvePoint(input_spline_inf_, u_test, true);
if(!std::isnan(curve_point.x)&&!std::isnan(curve_point.y))
posesOnEdge.push_back(Pose(curve_point.x, curve_point.y, 0));
}
@@ -490,11 +461,11 @@ namespace custom_planner
continue;
}
double angle_1 = calculateAngle(posesOnEdge.front().getX(),posesOnEdge.front().getY(),
double angle_1 = merge_path_calc_.calculateAngle(posesOnEdge.front().getX(),posesOnEdge.front().getY(),
posesOnEdge[(int)(posesOnEdge.size()/2)].getX(),
posesOnEdge[(int)(posesOnEdge.size()/2)].getY());
double angle_2 = calculateAngle(posesOnEdge[(int)(posesOnEdge.size()/2)].getX(),
double angle_2 = merge_path_calc_.calculateAngle(posesOnEdge[(int)(posesOnEdge.size()/2)].getX(),
posesOnEdge[(int)(posesOnEdge.size()/2)].getY(),
posesOnEdge.back().getX(),posesOnEdge.back().getY());
@@ -504,19 +475,19 @@ namespace custom_planner
if(edges_info_.back().isCurve || delta_angle > EPSILON)
for (int idx_segment_A = 0; idx_segment_A + 1 < (int)savePosesOnEdge.size(); ++idx_segment_A)
{
geometry_msgs::Point gp1 ;
robot_geometry_msgs::Point gp1 ;
gp1.x = savePosesOnEdge[idx_segment_A].getX();
gp1.y = savePosesOnEdge[idx_segment_A].getY();
geometry_msgs::Point gp2;
robot_geometry_msgs::Point gp2;
gp2.x = savePosesOnEdge[idx_segment_A + 1].getX();
gp2.y = savePosesOnEdge[idx_segment_A + 1].getY();
for (int idx_segment_B = (int)posesOnEdge.size() - 1; idx_segment_B > 0; --idx_segment_B)
{
geometry_msgs::Point lp1;
robot_geometry_msgs::Point lp1;
lp1.x = posesOnEdge[idx_segment_B].getX();
lp1.y = posesOnEdge[idx_segment_B].getY();
geometry_msgs::Point lp2;
robot_geometry_msgs::Point lp2;
lp2.x = posesOnEdge[idx_segment_B - 1].getX();
lp2.y = posesOnEdge[idx_segment_B - 1].getY();
@@ -528,7 +499,7 @@ namespace custom_planner
auto intersection_point = computeIntersectionPoint(gp1, gp2, lp1, lp2);
if (intersection_point)
{
geometry_msgs::Point p = intersection_point.value();
robot_geometry_msgs::Point p = intersection_point.value();
// savePosesOnEdge.push_back(Pose(p.x, p.y, 0));
posesOnEdge.insert(posesOnEdge.begin(), Pose(p.x, p.y, 0));
}
@@ -613,10 +584,10 @@ namespace custom_planner
return status_calcAllYaw;
}
bool CustomPlanner::doIntersect(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2)
bool CustomPlanner::doIntersect(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2)
{
auto orientation = [](const geometry_msgs::Point& a, const geometry_msgs::Point& b, const geometry_msgs::Point& c)
auto orientation = [](const robot_geometry_msgs::Point& a, const robot_geometry_msgs::Point& b, const robot_geometry_msgs::Point& c)
{
double val = (b.y - a.y) * (c.x - b.x) -
(b.x - a.x) * (c.y - b.y);
@@ -632,8 +603,8 @@ namespace custom_planner
return (o1 != o2 && o3 != o4);
}
std::optional<geometry_msgs::Point> CustomPlanner::computeIntersectionPoint(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2)
std::optional<robot_geometry_msgs::Point> CustomPlanner::computeIntersectionPoint(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2)
{
double x1 = p1.x, y1 = p1.y;
double x2 = q1.x, y2 = q1.y;
@@ -659,7 +630,7 @@ namespace custom_planner
if (between(x1, x2, x) && between(y1, y2, y) &&
between(x3, x4, x) && between(y3, y4, y))
{
geometry_msgs::Point pt;
robot_geometry_msgs::Point pt;
pt.x = x;
pt.y = y;
pt.z = 0.0;
@@ -799,8 +770,8 @@ namespace custom_planner
return true;
}
bool CustomPlanner::calcAllYaw(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
bool CustomPlanner::calcAllYaw(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
vector<Pose>& savePosesOnEdge,
int& total_edge)
{
@@ -815,7 +786,7 @@ namespace custom_planner
return false;
}
if(!(int)posesOnPathWay.empty()) posesOnPathWay.clear();
if(!(int)posesOnPathWay_.empty()) posesOnPathWay_.clear();
merge_path_calc_.status_yaw_edge_end_ = checkYawEdgeEnd(savePosesOnEdge[(int)savePosesOnEdge.size()-2],
savePosesOnEdge[(int)savePosesOnEdge.size()-1], goal);
@@ -908,7 +879,7 @@ namespace custom_planner
}
}
}
// posesOnPathWay.insert(posesOnPathWay.end(), Pose_goal_tmp.begin(),Pose_goal_tmp.end());
// posesOnPathWay_.insert(posesOnPathWay_.end(), Pose_goal_tmp.begin(),Pose_goal_tmp.end());
}
else
{
@@ -975,7 +946,7 @@ namespace custom_planner
savePoseTMP.insert(savePoseTMP.begin() + insert_pos, MatrixPose[i].begin(), MatrixPose[i].end());
}
}
posesOnPathWay.insert(posesOnPathWay.end(),
posesOnPathWay_.insert(posesOnPathWay_.end(),
savePoseTMP.begin(),
savePoseTMP.end());
}
@@ -1024,7 +995,7 @@ namespace custom_planner
}
MatrixPose[i].shrink_to_fit();
}
posesOnPathWay.insert(posesOnPathWay.end(),
posesOnPathWay_.insert(posesOnPathWay_.end(),
savePosesOnEdge.begin(),
savePosesOnEdge.end());
}
@@ -1034,12 +1005,12 @@ namespace custom_planner
int nearest_idx = -1;
double min_dist = std::numeric_limits<double>::max();
int start_idx = edges_info_[0].end_edge_idx;
int end_idx = std::min(edges_info_[1].end_edge_idx, (int)posesOnPathWay.size());
int end_idx = std::min(edges_info_[1].end_edge_idx, (int)posesOnPathWay_.size());
for (int i = start_idx; i < end_idx; i++)
{
double dx = start.pose.position.x - posesOnPathWay[i].getX();
double dy = start.pose.position.y - posesOnPathWay[i].getY();
double dx = start.pose.position.x - posesOnPathWay_[i].getX();
double dy = start.pose.position.y - posesOnPathWay_[i].getY();
double dist = std::hypot(dx, dy);
if (dist < min_dist)
@@ -1049,17 +1020,17 @@ namespace custom_planner
}
}
if (nearest_idx > 0 && nearest_idx < (int)posesOnPathWay.size())
if (nearest_idx > 0 && nearest_idx < (int)posesOnPathWay_.size())
{
posesOnPathWay.erase(posesOnPathWay.begin(), posesOnPathWay.begin() + nearest_idx);
posesOnPathWay_.erase(posesOnPathWay_.begin(), posesOnPathWay_.begin() + nearest_idx);
merge_path_calc_.edge_front_start_idx_ = 0;
merge_path_calc_.edge_front_end_idx_ = end_idx - nearest_idx;
int total = edges_info_.back().start_edge_idx - edges_info_.back().end_edge_idx;
merge_path_calc_.edge_back_end_idx_ = (int)posesOnPathWay.size() - 1;
merge_path_calc_.edge_back_end_idx_ = (int)posesOnPathWay_.size() - 1;
merge_path_calc_.edge_back_start_idx_ = merge_path_calc_.edge_back_end_idx_ - total;
// posesOnPathWay.shrink_to_fit();
// printf("[custom_planner][makePlanWithOrder] DEBUG: 2 posesOnPathWay size(%d)", (int)posesOnPathWay.size());
// posesOnPathWay_.shrink_to_fit();
// printf("[custom_planner][makePlanWithOrder] DEBUG: 2 posesOnPathWay_ size(%d)", (int)posesOnPathWay_.size());
}
}
return true;
@@ -1130,7 +1101,7 @@ namespace custom_planner
if(posesOnEdge.size()>2){
for(int i = 0; i<((int)posesOnEdge.size()-1); i++)
{
double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
double theta = merge_path_calc_.calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
posesOnEdge[i+1].getX(), posesOnEdge[i+1].getY());
posesOnEdge[i].setYaw(theta);
}
@@ -1140,7 +1111,7 @@ namespace custom_planner
{
if(posesOnEdge[0].getX()!=posesOnEdge[1].getX())
{
double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(),
double theta = merge_path_calc_.calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(),
posesOnEdge[1].getX(), posesOnEdge[1].getY());
posesOnEdge[0].setYaw(theta);
posesOnEdge[1].setYaw(theta);
@@ -1154,7 +1125,7 @@ namespace custom_planner
if(posesOnEdge.size()>2){
for(int i = (int)posesOnEdge.size() -1; i>0; i--)
{
double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
double theta = merge_path_calc_.calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
posesOnEdge[i-1].getX(), posesOnEdge[i-1].getY());
posesOnEdge[i].setYaw(theta);
}
@@ -1164,7 +1135,7 @@ namespace custom_planner
{
if(posesOnEdge[1].getX()!=posesOnEdge[0].getX())
{
double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(),
double theta = merge_path_calc_.calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(),
posesOnEdge[0].getX(), posesOnEdge[0].getY());
posesOnEdge[1].setYaw(theta);
posesOnEdge[0].setYaw(theta);
@@ -1174,22 +1145,29 @@ namespace custom_planner
}
}
bool CustomPlanner::checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, const geometry_msgs::PoseStamped& goal)
bool CustomPlanner::checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, const robot_geometry_msgs::PoseStamped& goal)
{
double angle = calculateAngle(start_edge_pose.getX(), start_edge_pose.getY(), end_edge_pose.getX(), end_edge_pose.getY());
double angle = merge_path_calc_.calculateAngle(start_edge_pose.getX(), start_edge_pose.getY(), end_edge_pose.getX(), end_edge_pose.getY());
double yaw_goal = data_convert::getYaw(goal.pose.orientation);
// printf("[custom_planner] DEBUG: angle: %f; yaw_goal: %f",angle, yaw_goal);
double delta_angle = fabs((merge_path_calc_.normalizeAngle(yaw_goal)) - (merge_path_calc_.normalizeAngle(angle)));
auto normalizeAngle0To2Pi = [](double angle) -> double
{
angle = fmod(angle, 2.0 * M_PI);
if (angle < 0)
angle += 2.0 * M_PI;
return angle;
};
double delta_angle = fabs((normalizeAngle0To2Pi(yaw_goal)) - (normalizeAngle0To2Pi(angle)));
return (delta_angle > (0.5*M_PI));
}
// Export factory function
static boost::shared_ptr<nav_core::BaseGlobalPlanner> custom_planner_plugin() {
static boost::shared_ptr<robot_nav_core::BaseGlobalPlanner> custom_planner_plugin() {
return boost::make_shared<custom_planner::CustomPlanner>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(custom_planner_plugin, custom_planner)
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(custom_planner::custom_planner_plugin, CustomPlanner)

0
src/custom_planner_config.cpp Normal file
View File

75
src/merge_path_calc.cpp
View File

@@ -12,10 +12,10 @@ namespace custom_planner {
delete spline_inf;
}
std::vector<geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<geometry_msgs::PoseStamped>& control_points, bool reverse)
std::vector<robot_geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<robot_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
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path;
std::vector<robot_geometry_msgs::PoseStamped> saved_poses; // Lưu các pose đã tính toán
if((int)nurbs_path.size() > 0)
{
@@ -61,10 +61,10 @@ namespace custom_planner {
for(double t = 0.0; t <= 1.0; t += step)
{
geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
robot_geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
if(!std::isnan(point.x) && !std::isnan(point.y))
{
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = robot::Time::now();
pose.pose.position = point;
pose.pose.orientation.x = 0.0;
@@ -112,7 +112,7 @@ namespace custom_planner {
double initial_step)
{
double length = 0.0;
geometry_msgs::Point prev_point, curr_point;
robot_geometry_msgs::Point prev_point, curr_point;
std::vector<double> segment_lengths;
static double step;
@@ -137,8 +137,8 @@ namespace custom_planner {
return length;
}
void MergePathCalc::updatePoseOrientation(std::vector<geometry_msgs::PoseStamped>& saved_poses,
std::vector<geometry_msgs::PoseStamped>& nurbs_path,
void MergePathCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
bool reverse)
{
double yaw;
@@ -185,8 +185,10 @@ namespace custom_planner {
if(type == START)
{
// int idx_check = edge_front_end_idx_;
int idx_check = static_cast<int>(posesOnPathWay.size());
// 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())) {
if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
return -1;
}
@@ -194,7 +196,7 @@ namespace custom_planner {
double min_dist = std::numeric_limits<double>::max();
double distance;
for (int i = 0; i < edge_front_end_idx_; ++i)
for (int i = 0; i < idx_check; ++i)
{
distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(),
posesOnPathWay[i].getY() - pose.getY());
@@ -221,10 +223,10 @@ namespace custom_planner {
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++)
for(int i = store_start_nearest_idx_; i <= idx_check; i++)
{
// Bounds checking đầy đủ trước khi truy cập
if (i + 1 < static_cast<int>(posesOnPathWay.size()) && i + 1 <= edge_front_end_idx_)
if (i + 1 < static_cast<int>(posesOnPathWay.size()) && i + 1 <= idx_check)
{
double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i + 1]);
@@ -236,7 +238,7 @@ namespace custom_planner {
}
if(i >= edge_front_end_idx_ - 1)
if(i >= idx_check - 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
@@ -312,8 +314,8 @@ namespace custom_planner {
return nearest_idx;
}
bool MergePathCalc::findNURBSControlPoints(vector<geometry_msgs::PoseStamped>& control_points,
const geometry_msgs::PoseStamped& pose,
bool MergePathCalc::findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
const robot_geometry_msgs::PoseStamped& pose,
std::vector<Pose>& posesOnPathWay,
point_type type)
{
@@ -332,15 +334,18 @@ namespace custom_planner {
int idx = findNearestPointOnPath(start_pose, posesOnPathWay, START);
// int idx_check = edge_front_end_idx_;
int idx_check = static_cast<int>(posesOnPathWay.size());
// 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())) {
// FIX: Validate idx_check trước khi sử dụng
if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
return false;
}
start_target_idx_ = edge_front_end_idx_ - 1;
start_target_idx_ = idx_check - 1;
double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();
@@ -349,7 +354,7 @@ namespace custom_planner {
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;
robot_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;
@@ -358,7 +363,7 @@ namespace custom_planner {
control_points.push_back(mid_pose);
// Thêm điểm cuối
geometry_msgs::PoseStamped end_pose;
robot_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());
@@ -376,7 +381,7 @@ namespace custom_planner {
// Tạo các control point cho NURBS
control_points.push_back(pose);
geometry_msgs::PoseStamped mid_pose;
robot_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;
@@ -393,7 +398,7 @@ namespace custom_planner {
// 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++)
for(int i = start_target_idx_; i <= idx_check; i++)
{
// FIX: Validate bounds trước khi truy cập
if (i >= static_cast<int>(posesOnPathWay.size())) {
@@ -409,7 +414,7 @@ namespace custom_planner {
found_suitable_point = true;
break;
}
if(i == edge_front_end_idx_)
if(i == idx_check)
{
// FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
found_suitable_point = false;
@@ -422,26 +427,28 @@ namespace custom_planner {
// Clear và tạo straight line thay vì goto
control_points.clear();
// int idx_check = edge_front_end_idx_;
int idx_check = static_cast<int>(posesOnPathWay.size());
// FIX: Validate lại edge_front_end_idx_
if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast<int>(posesOnPathWay.size())) {
if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
return false;
}
start_target_idx_ = edge_front_end_idx_ - 1;
start_target_idx_ = idx_check - 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;
robot_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;
robot_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());
@@ -451,7 +458,7 @@ namespace custom_planner {
return true;
}
geometry_msgs::PoseStamped end_pose;
robot_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);
@@ -482,7 +489,7 @@ namespace custom_planner {
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
// Thêm điểm đầu từ đường đã có
geometry_msgs::PoseStamped start_pose;
robot_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());
@@ -495,7 +502,7 @@ namespace custom_planner {
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;
robot_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;
@@ -553,7 +560,7 @@ namespace custom_planner {
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
geometry_msgs::PoseStamped start_pose_straight;
robot_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());
@@ -563,7 +570,7 @@ namespace custom_planner {
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;
robot_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;
@@ -575,7 +582,7 @@ namespace custom_planner {
}
// Thêm điểm đầu từ đường đã có
geometry_msgs::PoseStamped start_pose;
robot_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());
@@ -583,7 +590,7 @@ namespace custom_planner {
start_pose.header = pose.header;
control_points.push_back(start_pose);
geometry_msgs::PoseStamped mid_pose;
robot_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;

50
src/plan_for_retry.cpp
View File

@@ -9,7 +9,7 @@
#include <tf2/convert.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
using namespace std;
@@ -41,7 +41,7 @@ bool isThetaValid(double theta)
return result;
}
double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose)
double computeDeltaAngleStartOfPlan(double theta, robot_geometry_msgs::Pose& startPose, robot_geometry_msgs::Pose& next_Pose)
{
double delta_angle = 0;
if(isThetaValid(theta))
@@ -66,7 +66,7 @@ double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose
return delta_angle;
}
double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose)
double computeDeltaAngleEndOfPlan(double theta, robot_geometry_msgs::Pose& endPose, robot_geometry_msgs::Pose& prev_Pose)
{
double delta_angle = 0;
if(isThetaValid(theta))
@@ -92,8 +92,8 @@ double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, ge
}
// 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;
std::vector<robot_geometry_msgs::PoseStamped> divideSegment(robot_geometry_msgs::PoseStamped& A, robot_geometry_msgs::PoseStamped& B, double d) {
std::vector<robot_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);
@@ -106,7 +106,7 @@ std::vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped
// 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;
robot_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;
@@ -181,14 +181,14 @@ std::vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped
// 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 makePlanForRetry(std::vector<robot_geometry_msgs::PoseStamped>& current_plan,
int indexOfPoseA, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_B_behind,
robot_geometry_msgs::PoseStamped& pose_C, std::vector<robot_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;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_1;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_2;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_3;
if(current_plan.empty()||current_plan.size()<2)
{
@@ -196,7 +196,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
return false;
}
geometry_msgs::PoseStamped pose_A;
robot_geometry_msgs::PoseStamped pose_A;
pose_A = current_plan[indexOfPoseA];
// Tính ra PlanRetry_1 điểm retry tại Pose_A
@@ -228,7 +228,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
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;
std::vector<robot_geometry_msgs::PoseStamped> planSegment_AB;
planSegment_AB = divideSegment(pose_A, pose_B, 0.1);
PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end());
}
@@ -285,7 +285,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
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;
robot_geometry_msgs::PoseStamped p;
p.pose.position.x = xP;
p.pose.position.y = yP;
p.pose.position.z = 0;
@@ -299,7 +299,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
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;
robot_geometry_msgs::PoseStamped p;
p.pose.position.x = xP;
p.pose.position.y = yP;
p.pose.position.z = 0;
@@ -420,13 +420,13 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
// 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 makePlanForRetry(robot_geometry_msgs::PoseStamped& pose_A, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_B_behind,
robot_geometry_msgs::PoseStamped& pose_C, std::vector<robot_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;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_1;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_2;
std::vector<robot_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,
@@ -459,7 +459,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta
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;
std::vector<robot_geometry_msgs::PoseStamped> planSegment_AB;
planSegment_AB = divideSegment(pose_A, pose_B, 0.1);
PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end());
}
@@ -516,7 +516,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta
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;
robot_geometry_msgs::PoseStamped p;
p.pose.position.x = xP;
p.pose.position.y = yP;
p.pose.position.z = 0;
@@ -530,7 +530,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta
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;
robot_geometry_msgs::PoseStamped p;
p.pose.position.x = xP;
p.pose.position.y = yP;
p.pose.position.z = 0;
@@ -648,7 +648,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta
// 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)
bool findCenterOfCurve(robot_geometry_msgs::PoseStamped& pose_A, robot_geometry_msgs::PoseStamped& pose_B, robot_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),