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.0.2)
cmake_minimum_required(VERSION 3.10) project(custom_planner VERSION 1.0.0 LANGUAGES CXX)
project(custom_planner)
# --- C++ standard và position independent code --- if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(CMAKE_CXX_STANDARD 17) # Sử dụng C++17 set(BUILDING_WITH_CATKIN TRUE)
set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Thư viện có thể build thành shared lib message(STATUS "Building custom_planner with Catkin")
# --- RPATH settings: ưu tiên thư viện build tại chỗ --- else()
# Dùng để runtime linker tìm thư viện đã build trước khi install set(BUILDING_WITH_CATKIN FALSE)
set(CMAKE_SKIP_BUILD_RPATH FALSE) message(STATUS "Building custom_planner with Standalone CMake")
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) endif()
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 --- # C++ Standard - must be set before find_package
# Tìm các thư viện cần thiết set(CMAKE_CXX_STANDARD 17)
# find_package(tf3 REQUIRED) # Nếu dùng tf3 set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(Eigen3 REQUIRED) # Thư viện Eigen cho toán học set(CMAKE_CXX_EXTENSIONS OFF)
find_package(Boost REQUIRED COMPONENTS system thread filesystem) # Boost: system, thread, filesystem
# --- Include directories --- # Find dependencies
# Thêm các folder chứa header files find_package(Eigen3 REQUIRED)
include_directories( 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 include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
) )
endif()
# --- Eigen và PCL definitions --- # ========================================================
add_definitions(${EIGEN3_DEFINITIONS}) # Libraries
# ========================================================
# --- Core library: custom_planner --- add_library(${PROJECT_NAME} SHARED
# Tạo thư viện chính
add_library(custom_planner
src/custom_planner.cpp src/custom_planner.cpp
src/pathway.cc src/pathway.cc
src/pose.cc src/pose.cc
@@ -41,70 +91,90 @@ add_library(custom_planner
src/merge_path_calc.cpp src/merge_path_calc.cpp
) )
# --- Link các thư viện phụ thuộc --- if(BUILDING_WITH_CATKIN)
target_link_libraries(custom_planner add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
${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(${PROJECT_NAME}
target_include_directories(custom_planner
PUBLIC PUBLIC
${Boost_INCLUDE_DIRS} # Boost headers $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> # Khi build từ source $<INSTALL_INTERFACE:include>
$<INSTALL_INTERFACE:include/${PROJECT_NAME}> # Khi install
) )
# --- Cài đặt thư viện vào hệ thống khi chạy make install --- target_link_libraries(${PROJECT_NAME}
install(TARGETS custom_planner PUBLIC ${catkin_LIBRARIES}
EXPORT custom_planner-targets PRIVATE Eigen3::Eigen Boost::system Boost::thread Boost::filesystem
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 --- else()
# --- 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 --- target_include_directories(${PROJECT_NAME}
install(DIRECTORY include/${PROJECT_NAME}/ PUBLIC
DESTINATION include/${PROJECT_NAME} $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
) $<INSTALL_INTERFACE:include>
)
# # --- Plugin libraries --- target_link_libraries(${PROJECT_NAME}
# # Tạo các plugin shared library PUBLIC
# add_library(plugins ${PACKAGES_DIR}
# SHARED PRIVATE
# plugins/static_layer.cpp Eigen3::Eigen Boost::system Boost::thread Boost::filesystem
# 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 set_target_properties(${PROJECT_NAME} PROPERTIES
# custom_planner LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
# ${Boost_LIBRARIES} BUILD_RPATH "${CMAKE_BINARY_DIR}"
# yaml-cpp INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
# robot_time )
# )
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 <deque>
// #include <iostream> // #include <iostream>
#include <nav_msgs/Path.h> #include <robot_nav_msgs/Path.h>
#include <geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h> #include <robot_visualization_msgs/Marker.h>
#include "custom_planner/color.h" #include "custom_planner/color.h"
struct Spline_Inf struct Spline_Inf
@@ -47,16 +47,16 @@ class Curve_common
{ {
public: public:
Curve_common(); Curve_common();
nav_msgs::Path Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, 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);
nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_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);
nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id); robot_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); robot_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); robot_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_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); 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); robot_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 CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS);
double CalculateCurvature(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); double CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
Eigen::Vector3d CalculateCurvatureDirectionVector(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 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(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 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(robot_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, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
//TODO: move relate visualize function to new vislization.h //TODO: move relate visualize function to new vislization.h
int print(); int print();
visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, 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);
visualization_msgs::Marker 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 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 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 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 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: private:

6
include/custom_planner/color.h
View File

@@ -1,14 +1,14 @@
#ifndef CUSTOM_PLANNER_COLOR_H_ #ifndef CUSTOM_PLANNER_COLOR_H_
#define CUSTOM_PLANNER_COLOR_H_ #define CUSTOM_PLANNER_COLOR_H_
#include <std_msgs/ColorRGBA.h> #include <robot_std_msgs/ColorRGBA.h>
namespace agv_visualization namespace agv_visualization
{ {
class Color : public std_msgs::ColorRGBA class Color : public robot_std_msgs::ColorRGBA
{ {
public: 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) : Color(red, green, blue, 1.0) {}
Color(double red, double green, double blue, double alpha) : Color() { Color(double red, double green, double blue, double alpha) : Color() {
r = red; r = red;

10
include/custom_planner/conversion.h
View File

@@ -3,14 +3,14 @@
#include "custom_planner/Curve_common.h" #include "custom_planner/Curve_common.h"
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.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); 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::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point; EigenTrajectoryPoint eigen_trajectory_point;
@@ -25,7 +25,7 @@ inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::v
return eigen_trajectory_point_vec; 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::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point; EigenTrajectoryPoint eigen_trajectory_point;

123
include/custom_planner/custom_planner.h
View File

@@ -9,14 +9,14 @@
using namespace std; using namespace std;
// ROS // ROS
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h> #include <robot_visualization_msgs/Marker.h>
// Costmap used for the map representation // Costmap used for the map representation
#include <costmap_2d/costmap_2d_robot.h> #include <robot_costmap_2d/costmap_2d_robot.h>
// global representation // global representation
#include <nav_core/base_global_planner.h> #include <robot_nav_core/base_global_planner.h>
// tf // tf
#include <tf3/convert.h> #include <tf3/convert.h>
@@ -29,18 +29,8 @@ using namespace std;
#include "custom_planner/conversion.h" #include "custom_planner/conversion.h"
#include "custom_planner/merge_path_calc.h" #include "custom_planner/merge_path_calc.h"
#include <geometry_msgs/PoseArray.h> #include <robot_geometry_msgs/PoseArray.h>
#include <thread>
#include <boost/thread.hpp>
#include <robot_protocol_msgs/Order.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; const double EPSILON = 1e-6;
@@ -56,7 +46,7 @@ struct OrderNode{
double theta; double theta;
}; };
class CustomPlanner : public nav_core::BaseGlobalPlanner{ class CustomPlanner : public robot_nav_core::BaseGlobalPlanner{
public: public:
/** /**
@@ -70,7 +60,7 @@ public:
* @param name The name of this planner * @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use * @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 * @param costmap_ros A pointer to the ROS wrapper of the costmap to use
*/ */
virtual bool initialize(std::string name, 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, virtual bool makePlan(const robot_protocol_msgs::Order& msg,
const geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan); std::vector<robot_geometry_msgs::PoseStamped>& plan);
/** /**
* @brief Given a goal pose in the world, compute a plan * @brief Given a goal pose in the world, compute a plan
* @param start The start pose * @param start The start pose
@@ -92,9 +82,9 @@ public:
* @param plan The plan... filled by the planner * @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise * @return True if a valid plan was found, false otherwise
*/ */
virtual bool makePlan(const geometry_msgs::PoseStamped& start, virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan) std::vector<robot_geometry_msgs::PoseStamped>& plan)
{ {
printf("[%s:%d] This function is not available!",__FILE__,__LINE__); printf("[%s:%d] This function is not available!",__FILE__,__LINE__);
return false; return false;
@@ -104,33 +94,24 @@ public:
private: private:
void publishStats(int solution_cost, int solution_size, void publishStats(int solution_cost, int solution_size,
const geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal); const robot_geometry_msgs::PoseStamped& goal);
static void transformFootprintToEdges(const geometry_msgs::Pose& robot_pose, static void transformFootprintToEdges(const robot_geometry_msgs::Pose& robot_pose,
const std::vector<geometry_msgs::Point>& footprint, const std::vector<robot_geometry_msgs::Point>& footprint,
std::vector<geometry_msgs::Point>& out_footprint); std::vector<robot_geometry_msgs::Point>& out_footprint);
Pose pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length = 1.0); Pose pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length = 1.0);
bool countRobotDirectionChangeAngle(vector<Pose>& savePosesOnEdge, int& total_edge); 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) inline bool isOppositeSign(double angleA, double angleB)
{ {
return (angleA > 0 && angleB < 0) || (angleA < 0 && angleB > 0); return (angleA > 0 && angleB < 0) || (angleA < 0 && angleB > 0);
} }
bool checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, 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); bool loadPathwayData(const string& filename);
@@ -139,17 +120,17 @@ private:
// void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg); // void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg);
bool makePlanWithOrder(robot_protocol_msgs::Order msg, bool makePlanWithOrder(robot_protocol_msgs::Order msg,
const geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal); const robot_geometry_msgs::PoseStamped& goal);
bool doIntersect(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1, bool doIntersect(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2); 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, std::optional<robot_geometry_msgs::Point> computeIntersectionPoint(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2); const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2);
bool calcAllYaw(const geometry_msgs::PoseStamped& start, bool calcAllYaw(const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
vector<Pose>& saveposesOnEdge, vector<Pose>& saveposesOnEdge,
int& total_edge); int& total_edge);
@@ -163,7 +144,7 @@ private:
double computeDeltaAngle(Pose& Pose1, Pose& Pose2); double computeDeltaAngle(Pose& Pose1, Pose& Pose2);
vector<Pose> divideSegment(Pose& A, Pose& B, double d); 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 // Vector lưu thông tin các cạnh
std::vector<InfEdge> edges_info_; std::vector<InfEdge> edges_info_;
@@ -173,55 +154,19 @@ private:
bool skipEdge_flag_ = false; bool skipEdge_flag_ = false;
bool reverse_ = false; bool reverse_ = false;
Spline_Inf* input_spline_inf; Spline_Inf* input_spline_inf_;
Curve_common* CurveDesign; Curve_common* CurveDesign_;
Pathway* pathway; vector<Pose> posesOnPathWay_;
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 initialized_;
bool rotating_robot_plag_ = false; /**< whether the robot is rotating or not, used to determine the direction of the path */ 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_; std::string name_;
costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */ robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
std::vector<geometry_msgs::Point> footprint_; std::vector<robot_geometry_msgs::Point> footprint_;
unsigned int current_env_width_; unsigned int current_env_width_;
unsigned int current_env_height_; 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_; 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_ #ifndef MERGER_PATH_CALC_H_
#define MERGER_PATH_CALC_H_ #define MERGER_PATH_CALC_H_
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <vector> #include <vector>
#include <cmath> #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. * 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. * 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, bool findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
const geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& start,
std::vector<Pose>& posesOnPathWay, std::vector<Pose>& posesOnPathWay,
point_type type); 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. * 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. * 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 reverse);
bool handleEdgeIntersection(vector<Pose>& savePosesOnEdge, bool handleEdgeIntersection(vector<Pose>& savePosesOnEdge,
@@ -174,7 +174,7 @@ namespace custom_planner
private: private:
// vector<geometry_msgs::PoseStamped> control_points; // vector<robot_geometry_msgs::PoseStamped> control_points;
bool normal_plag = false; 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. * 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. * 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, void updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
std::vector<geometry_msgs::PoseStamped>& nurbs_path, std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
bool reverse); 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à 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. * 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); 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/Curve_common.h"
#include "custom_planner/conversion.h" #include "custom_planner/conversion.h"
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <iostream> #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; robot_nav_msgs::Path line_result;
geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
line_result.header.frame_id = frame_id; line_result.header.frame_id = frame_id;
line_result.header.stamp = robot::Time::now(); 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; 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; robot_nav_msgs::Path bezier_curve_result;
geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
EigenTrajectoryPoint::Vector temp_control_point_vec; EigenTrajectoryPoint::Vector temp_control_point_vec;
bezier_curve_result.header.frame_id = frame_id; 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.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now(); 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 = color;
waypoints_marker.color.r = 1; waypoints_marker.color.r = 1;
waypoints_marker.color.g = 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())); 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++) for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{ {
view_point.x = discreate_point.at(i)(0); 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; 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.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now(); 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 = point_color;
waypoints_marker.color.a = 0.75; waypoints_marker.color.a = 0.75;
waypoints_marker.ns = name; 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())); 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++) for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{ {
view_point.x = discreate_point.at(i)(0); view_point.x = discreate_point.at(i)(0);
@@ -192,13 +192,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector<Eigen::
return waypoints_marker; 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.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now(); 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 = color;
waypoints_marker.color.r = 1; waypoints_marker.color.r = 1;
waypoints_marker.color.g = 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())); 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++) for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{ {
view_point.x = discreate_point.at(i).position(0); view_point.x = discreate_point.at(i).position(0);
@@ -221,13 +221,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint
return waypoints_marker; 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.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now(); 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 = point_color;
// waypoints_marker.color.r = r; // waypoints_marker.color.r = r;
// waypoints_marker.color.g = g; // 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())); 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++) for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{ {
view_point.x = discreate_point.at(i).position(0); view_point.x = discreate_point.at(i).position(0);
@@ -251,9 +251,9 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoin
return waypoints_marker; 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++) for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{ {
view_point.x = discreate_point.at(i).position(0); 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++) for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{ {
view_point.x = discreate_point.at(i)(0); 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; robot_nav_msgs::Path bspline_curve_result;
geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
bspline_curve_result.header.frame_id = frame_id; bspline_curve_result.header.frame_id = frame_id;
bspline_curve_result.header.stamp = robot::Time::now(); 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; 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; robot_nav_msgs::Path nurbs_curve_result;
geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
nurbs_curve_result.header.frame_id = frame_id; nurbs_curve_result.header.frame_id = frame_id;
nurbs_curve_result.header.stamp = robot::Time::now(); 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; int p_degree = spline_inf->order - 1;
double sum_x = 0, sum_y = 0; double sum_x = 0, sum_y = 0;
double sum_denom = 0; double sum_denom = 0;
@@ -878,11 +878,11 @@ geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spl
return derivative_curve_point; 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; robot_geometry_msgs::Point derivative_point_result;
nav_msgs::Path bspline_derivative_result; robot_nav_msgs::Path bspline_derivative_result;
geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
bspline_derivative_result.header.frame_id = frame_id; bspline_derivative_result.header.frame_id = frame_id;
bspline_derivative_result.header.stamp = robot::Time::now(); 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; 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; robot_geometry_msgs::Point derivative_point_result;
nav_msgs::Path derivative_basis_result; robot_nav_msgs::Path derivative_basis_result;
geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
derivative_basis_result.header.frame_id = frame_id; derivative_basis_result.header.frame_id = frame_id;
derivative_basis_result.header.stamp = robot::Time::now(); 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; 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? //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 p_degree = spline_inf->order - 1;
int n = static_cast<int>(spline_inf->control_point.size()) - 1; int n = static_cast<int>(spline_inf->control_point.size()) - 1;
//TODO: Check knot vector size and sequence is correect //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 <custom_planner/custom_planner.h>
#include <nav_msgs/Path.h> #include <robot_nav_msgs/Path.h>
#include <robot_geometry_msgs/Point.h>
#include <costmap_2d/inflation_layer.h> #include <robot_costmap_2d/inflation_layer.h>
#include <tf3/LinearMath/Quaternion.h> #include <tf3/LinearMath/Quaternion.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using namespace std; 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 namespace custom_planner
{ {
CustomPlanner::CustomPlanner() 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) : initialized_(false), costmap_robot_(NULL)
{ {
initialize(name, costmap_robot); 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_) if (!initialized_)
{ {
@@ -36,31 +28,8 @@ namespace custom_planner
robot::NodeHandle p_nh("~"); robot::NodeHandle p_nh("~");
printf("Name is %s", name.c_str()); printf("Name is %s", name.c_str());
pathway = new Pathway(); input_spline_inf_ = new Spline_Inf();
startPose = new Pose(); CurveDesign_ = new Curve_common();
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));
name_ = name; name_ = name;
costmap_robot_ = costmap_robot; costmap_robot_ = costmap_robot;
@@ -69,13 +38,15 @@ namespace custom_planner
initialized_ = true; initialized_ = true;
printf("[custom_planner] Initialized successfully"); printf("[custom_planner] Initialized successfully");
return true;
} }
return false;
} }
bool CustomPlanner::makePlan(const robot_protocol_msgs::Order& msg, bool CustomPlanner::makePlan(const robot_protocol_msgs::Order& msg,
const geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan) std::vector<robot_geometry_msgs::PoseStamped>& plan)
{ {
plan.clear(); plan.clear();
@@ -99,8 +70,8 @@ namespace custom_planner
} }
// ===== STEP 2: VALIDATE PATHWAY ===== // ===== STEP 2: VALIDATE PATHWAY =====
if (posesOnPathWay.empty()) { if (posesOnPathWay_.empty()) {
printf("[custom_planner] posesOnPathWay is empty"); printf("[custom_planner] posesOnPathWay_ is empty");
return false; return false;
} }
@@ -114,9 +85,9 @@ namespace custom_planner
// ===== STEP 4: FIND NEAREST POINT ON PATHWAY ===== // ===== STEP 4: FIND NEAREST POINT ON PATHWAY =====
int nearest_idx = 0; int nearest_idx = 0;
double min_dist = std::numeric_limits<double>::max(); double min_dist = std::numeric_limits<double>::max();
for (int i = 0; i < static_cast<int>(posesOnPathWay.size()); ++i) { for (int i = 0; i < static_cast<int>(posesOnPathWay_.size()); ++i) {
double distance = std::hypot(posesOnPathWay[i].getX() - start.pose.position.x, double distance = std::hypot(posesOnPathWay_[i].getX() - start.pose.position.x,
posesOnPathWay[i].getY() - start.pose.position.y); posesOnPathWay_[i].getY() - start.pose.position.y);
if (distance < min_dist) { if (distance < min_dist) {
min_dist = distance; min_dist = distance;
nearest_idx = i; nearest_idx = i;
@@ -130,13 +101,13 @@ namespace custom_planner
robot::Time plan_time = robot::Time::now(); robot::Time plan_time = robot::Time::now();
// Calculate distance from start to nearest pathway point // Calculate distance from start to nearest pathway point
double dx_start = posesOnPathWay[nearest_idx].getX() - start.pose.position.x; double dx_start = posesOnPathWay_[nearest_idx].getX() - start.pose.position.x;
double dy_start = posesOnPathWay[nearest_idx].getY() - start.pose.position.y; 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); double d_start_to_path = std::sqrt(dx_start * dx_start + dy_start * dy_start);
// Calculate distance from pathway end to goal // Calculate distance from pathway end to goal
double dx_goal = goal.pose.position.x - posesOnPathWay.back().getX(); double dx_goal = goal.pose.position.x - posesOnPathWay_.back().getX();
double dy_goal = goal.pose.position.y - posesOnPathWay.back().getY(); 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); double d_path_to_goal = std::sqrt(dx_goal * dx_goal + dy_goal * dy_goal);
// ===== STEP 6: GENERATE PATH BASED ON DISTANCE CONDITIONS ===== // ===== 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 // CASE 1: Both start and goal are close to pathway - Direct path
printf("[custom_planner] Using direct pathway"); printf("[custom_planner] Using direct pathway");
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) { for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
geometry_msgs::PoseStamped pose; robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time; pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX(); pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY(); pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0; 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); plan.push_back(pose);
} }
@@ -160,17 +131,17 @@ namespace custom_planner
// CASE 2: Start is far, goal is close - NURBS from start to pathway // CASE 2: Start is far, goal is close - NURBS from start to pathway
printf("[custom_planner] Using start NURBS path reverse_(%x)", reverse_); 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( bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints(
start_control_points, start, posesOnPathWay, START); start_control_points, start, posesOnPathWay_, START);
if (valid_start_nurbs) { 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_); merge_path_calc_.calculateNURBSPath(start_control_points, reverse_);
// Add NURBS path // Add NURBS path
for (const auto& pose : 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.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0; new_pose.pose.position.z = 0;
@@ -178,26 +149,26 @@ namespace custom_planner
} }
// Add remaining pathway // Add remaining pathway
for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay.size(); i++) { for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay_.size(); i++) {
geometry_msgs::PoseStamped pose; robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time; pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX(); pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY(); pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0; 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); plan.push_back(pose);
} }
} else { } else {
// Fallback to direct path // Fallback to direct path
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) { for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
geometry_msgs::PoseStamped pose; robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time; pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX(); pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY(); pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0; 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); plan.push_back(pose);
} }
} }
@@ -206,29 +177,29 @@ namespace custom_planner
// CASE 3: Start is close, goal is far - NURBS from pathway to goal // CASE 3: Start is close, goal is far - NURBS from pathway to goal
printf("[custom_planner] Using goal NURBS path"); 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( bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints(
goal_control_points, goal, posesOnPathWay, GOAL); goal_control_points, goal, posesOnPathWay_, GOAL);
if (valid_goal_nurbs) { if (valid_goal_nurbs) {
// Add pathway up to goal connection point // Add pathway up to goal connection point
for (size_t i = nearest_idx; i < merge_path_calc_.goal_target_idx_; i++) { 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.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX(); pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY(); pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0; 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); plan.push_back(pose);
} }
// Add NURBS path to goal // 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_); merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
for (const auto& pose : 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.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0; new_pose.pose.position.z = 0;
@@ -236,14 +207,14 @@ namespace custom_planner
} }
} else { } else {
// Fallback: use entire pathway // Fallback: use entire pathway
for (size_t i = 0; i < posesOnPathWay.size(); i++) { for (size_t i = 0; i < posesOnPathWay_.size(); i++) {
geometry_msgs::PoseStamped pose; robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time; pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX(); pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY(); pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0; 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); plan.push_back(pose);
} }
} }
@@ -252,11 +223,11 @@ namespace custom_planner
// CASE 4: Both start and goal are far - Dual NURBS or special handling // CASE 4: Both start and goal are far - Dual NURBS or special handling
printf("[custom_planner] Using dual NURBS path"); 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( 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( 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_; 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]); costmap_robot_->getRobotPose(goal_control_points[0]);
goal_control_points[goal_control_points.size() - 1] = goal; 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_); merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
// Skip first point to avoid duplication // Skip first point to avoid duplication
for (size_t i = 1; i < nurbs_path.size(); i++) { 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.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0; new_pose.pose.position.z = 0;
@@ -281,10 +252,10 @@ namespace custom_planner
// Dual NURBS: Start NURBS + Pathway + Goal NURBS // Dual NURBS: Start NURBS + Pathway + Goal NURBS
// Add start NURBS path // Add start NURBS path
std::vector<geometry_msgs::PoseStamped> start_nurbs_path = std::vector<robot_geometry_msgs::PoseStamped> start_nurbs_path =
merge_path_calc_.calculateNURBSPath(start_control_points, false); merge_path_calc_.calculateNURBSPath(start_control_points, reverse_);
for (const auto& pose : start_nurbs_path) { 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.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0; new_pose.pose.position.z = 0;
@@ -292,22 +263,22 @@ namespace custom_planner
} }
// Add middle pathway segment // Add middle pathway segment
for (size_t i = (merge_path_calc_.start_target_idx_ - 1); i < merge_path_calc_.goal_target_idx_; i++) { for (size_t i = (merge_path_calc_.start_target_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.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX(); pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY(); pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0; 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); plan.push_back(pose);
} }
// Add goal NURBS path // 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_); merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
for (const auto& pose : goal_nurbs_path) { 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.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0; new_pose.pose.position.z = 0;
@@ -317,21 +288,21 @@ namespace custom_planner
} else { } else {
// Fallback: Direct pathway from nearest point // Fallback: Direct pathway from nearest point
printf("[custom_planner] NURBS control points not found, using fallback path"); printf("[custom_planner] NURBS control points not found, using fallback path");
for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) { for (size_t i = nearest_idx; i < posesOnPathWay_.size(); i++) {
geometry_msgs::PoseStamped pose; robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time; pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = posesOnPathWay[i].getX(); pose.pose.position.x = posesOnPathWay_[i].getX();
pose.pose.position.y = posesOnPathWay[i].getY(); pose.pose.position.y = posesOnPathWay_[i].getY();
pose.pose.position.z = 0; 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); plan.push_back(pose);
} }
} }
} }
// ===== STEP 7: ADD FINAL GOAL 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.stamp = plan_time;
pose_goal.header.frame_id = costmap_robot_->getGlobalFrameID(); pose_goal.header.frame_id = costmap_robot_->getGlobalFrameID();
pose_goal.pose = goal.pose; pose_goal.pose = goal.pose;
@@ -344,7 +315,7 @@ namespace custom_planner
} }
// Publish plan for visualization // 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.frame_id = costmap_robot_->getGlobalFrameID();
gui_path.header.stamp = plan_time; gui_path.header.stamp = plan_time;
gui_path.poses = plan; gui_path.poses = plan;
@@ -354,9 +325,9 @@ namespace custom_planner
return true; return true;
} }
void CustomPlanner::transformFootprintToEdges(const geometry_msgs::Pose &robot_pose, void CustomPlanner::transformFootprintToEdges(const robot_geometry_msgs::Pose &robot_pose,
const std::vector<geometry_msgs::Point> &footprint, const std::vector<robot_geometry_msgs::Point> &footprint,
std::vector<geometry_msgs::Point> &out_footprint) std::vector<robot_geometry_msgs::Point> &out_footprint)
{ {
out_footprint.resize(2 * footprint.size()); out_footprint.resize(2 * footprint.size());
double yaw = data_convert::getYaw(robot_pose.orientation); double yaw = data_convert::getYaw(robot_pose.orientation);
@@ -378,15 +349,15 @@ namespace custom_planner
} }
bool CustomPlanner::makePlanWithOrder(robot_protocol_msgs::Order msg, bool CustomPlanner::makePlanWithOrder(robot_protocol_msgs::Order msg,
const geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal) const robot_geometry_msgs::PoseStamped& goal)
{ {
vector<Pose> savePosesOnEdge; vector<Pose> savePosesOnEdge;
int count_two_curve_idx = 0; int count_two_curve_idx = 0;
//Xóa mảng nếu còn chứa phần tử không xác định //Xóa mảng nếu còn chứa phần tử không xác định
orderNodes.clear(); std::map<string, OrderNode> orderNodes;
posesOnPathWay.clear(); posesOnPathWay_.clear();
edges_info_.clear(); edges_info_.clear();
RobotDirectionChangeAngle_info_.clear(); RobotDirectionChangeAngle_info_.clear();
@@ -401,8 +372,8 @@ namespace custom_planner
if(distance_start_to_goal > 0.2) { if(distance_start_to_goal > 0.2) {
return false; return false;
} else { } else {
posesOnPathWay.push_back(Pose(start.pose.position.x, start.pose.position.y, data_convert::getYaw(start.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))); posesOnPathWay_.push_back(Pose(goal.pose.position.x, goal.pose.position.y, data_convert::getYaw(goal.pose.orientation)));
return true; return true;
} }
} }
@@ -460,14 +431,14 @@ namespace custom_planner
{ {
double t_intervel = 0.01; double t_intervel = 0.01;
order = degree + 1; order = degree + 1;
input_spline_inf->control_point.clear(); input_spline_inf_->control_point.clear();
input_spline_inf->knot_vector.clear(); input_spline_inf_->knot_vector.clear();
input_spline_inf->weight.clear(); input_spline_inf_->weight.clear();
CurveDesign->ReadSplineInf(input_spline_inf, order, control_points, knot_vector); CurveDesign_->ReadSplineInf(input_spline_inf_, order, control_points, knot_vector);
CurveDesign->ReadSplineInf(input_spline_inf, weight_vector, false); CurveDesign_->ReadSplineInf(input_spline_inf_, weight_vector, false);
//Tính độ dài của cạnh thứ i //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 //Tính step của cạnh thứ i
if (!costmap_robot_ || !costmap_robot_->getCostmap()) { if (!costmap_robot_ || !costmap_robot_->getCostmap()) {
return false; return false;
@@ -479,8 +450,8 @@ namespace custom_planner
//Tính các điểm theo step mới //Tính các điểm theo step mới
for(double u_test = 0; u_test <= 1; u_test += t_intervel_new) for(double u_test = 0; u_test <= 1; u_test += t_intervel_new)
{ {
geometry_msgs::Point curve_point; robot_geometry_msgs::Point curve_point;
curve_point = CurveDesign->CalculateCurvePoint(input_spline_inf, u_test, true); curve_point = CurveDesign_->CalculateCurvePoint(input_spline_inf_, u_test, true);
if(!std::isnan(curve_point.x)&&!std::isnan(curve_point.y)) if(!std::isnan(curve_point.x)&&!std::isnan(curve_point.y))
posesOnEdge.push_back(Pose(curve_point.x, curve_point.y, 0)); posesOnEdge.push_back(Pose(curve_point.x, curve_point.y, 0));
} }
@@ -490,11 +461,11 @@ namespace custom_planner
continue; 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)].getX(),
posesOnEdge[(int)(posesOnEdge.size()/2)].getY()); 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[(int)(posesOnEdge.size()/2)].getY(),
posesOnEdge.back().getX(),posesOnEdge.back().getY()); posesOnEdge.back().getX(),posesOnEdge.back().getY());
@@ -504,19 +475,19 @@ namespace custom_planner
if(edges_info_.back().isCurve || delta_angle > EPSILON) if(edges_info_.back().isCurve || delta_angle > EPSILON)
for (int idx_segment_A = 0; idx_segment_A + 1 < (int)savePosesOnEdge.size(); ++idx_segment_A) 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.x = savePosesOnEdge[idx_segment_A].getX();
gp1.y = savePosesOnEdge[idx_segment_A].getY(); 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.x = savePosesOnEdge[idx_segment_A + 1].getX();
gp2.y = savePosesOnEdge[idx_segment_A + 1].getY(); gp2.y = savePosesOnEdge[idx_segment_A + 1].getY();
for (int idx_segment_B = (int)posesOnEdge.size() - 1; idx_segment_B > 0; --idx_segment_B) 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.x = posesOnEdge[idx_segment_B].getX();
lp1.y = posesOnEdge[idx_segment_B].getY(); 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.x = posesOnEdge[idx_segment_B - 1].getX();
lp2.y = posesOnEdge[idx_segment_B - 1].getY(); lp2.y = posesOnEdge[idx_segment_B - 1].getY();
@@ -528,7 +499,7 @@ namespace custom_planner
auto intersection_point = computeIntersectionPoint(gp1, gp2, lp1, lp2); auto intersection_point = computeIntersectionPoint(gp1, gp2, lp1, lp2);
if (intersection_point) 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)); // savePosesOnEdge.push_back(Pose(p.x, p.y, 0));
posesOnEdge.insert(posesOnEdge.begin(), 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; return status_calcAllYaw;
} }
bool CustomPlanner::doIntersect(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1, bool CustomPlanner::doIntersect(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2) 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) - double val = (b.y - a.y) * (c.x - b.x) -
(b.x - a.x) * (c.y - b.y); (b.x - a.x) * (c.y - b.y);
@@ -632,8 +603,8 @@ namespace custom_planner
return (o1 != o2 && o3 != o4); return (o1 != o2 && o3 != o4);
} }
std::optional<geometry_msgs::Point> CustomPlanner::computeIntersectionPoint(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1, std::optional<robot_geometry_msgs::Point> CustomPlanner::computeIntersectionPoint(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2) const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2)
{ {
double x1 = p1.x, y1 = p1.y; double x1 = p1.x, y1 = p1.y;
double x2 = q1.x, y2 = q1.y; double x2 = q1.x, y2 = q1.y;
@@ -659,7 +630,7 @@ namespace custom_planner
if (between(x1, x2, x) && between(y1, y2, y) && if (between(x1, x2, x) && between(y1, y2, y) &&
between(x3, x4, x) && between(y3, y4, y)) between(x3, x4, x) && between(y3, y4, y))
{ {
geometry_msgs::Point pt; robot_geometry_msgs::Point pt;
pt.x = x; pt.x = x;
pt.y = y; pt.y = y;
pt.z = 0.0; pt.z = 0.0;
@@ -799,8 +770,8 @@ namespace custom_planner
return true; return true;
} }
bool CustomPlanner::calcAllYaw(const geometry_msgs::PoseStamped& start, bool CustomPlanner::calcAllYaw(const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
vector<Pose>& savePosesOnEdge, vector<Pose>& savePosesOnEdge,
int& total_edge) int& total_edge)
{ {
@@ -815,7 +786,7 @@ namespace custom_planner
return false; 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], merge_path_calc_.status_yaw_edge_end_ = checkYawEdgeEnd(savePosesOnEdge[(int)savePosesOnEdge.size()-2],
savePosesOnEdge[(int)savePosesOnEdge.size()-1], goal); 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 else
{ {
@@ -975,7 +946,7 @@ namespace custom_planner
savePoseTMP.insert(savePoseTMP.begin() + insert_pos, MatrixPose[i].begin(), MatrixPose[i].end()); savePoseTMP.insert(savePoseTMP.begin() + insert_pos, MatrixPose[i].begin(), MatrixPose[i].end());
} }
} }
posesOnPathWay.insert(posesOnPathWay.end(), posesOnPathWay_.insert(posesOnPathWay_.end(),
savePoseTMP.begin(), savePoseTMP.begin(),
savePoseTMP.end()); savePoseTMP.end());
} }
@@ -1024,7 +995,7 @@ namespace custom_planner
} }
MatrixPose[i].shrink_to_fit(); MatrixPose[i].shrink_to_fit();
} }
posesOnPathWay.insert(posesOnPathWay.end(), posesOnPathWay_.insert(posesOnPathWay_.end(),
savePosesOnEdge.begin(), savePosesOnEdge.begin(),
savePosesOnEdge.end()); savePosesOnEdge.end());
} }
@@ -1034,12 +1005,12 @@ namespace custom_planner
int nearest_idx = -1; int nearest_idx = -1;
double min_dist = std::numeric_limits<double>::max(); double min_dist = std::numeric_limits<double>::max();
int start_idx = edges_info_[0].end_edge_idx; 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++) for (int i = start_idx; i < end_idx; i++)
{ {
double dx = start.pose.position.x - posesOnPathWay[i].getX(); double dx = start.pose.position.x - posesOnPathWay_[i].getX();
double dy = start.pose.position.y - posesOnPathWay[i].getY(); double dy = start.pose.position.y - posesOnPathWay_[i].getY();
double dist = std::hypot(dx, dy); double dist = std::hypot(dx, dy);
if (dist < min_dist) 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_start_idx_ = 0;
merge_path_calc_.edge_front_end_idx_ = end_idx - nearest_idx; 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; 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; merge_path_calc_.edge_back_start_idx_ = merge_path_calc_.edge_back_end_idx_ - total;
// posesOnPathWay.shrink_to_fit(); // posesOnPathWay_.shrink_to_fit();
// printf("[custom_planner][makePlanWithOrder] DEBUG: 2 posesOnPathWay size(%d)", (int)posesOnPathWay.size()); // printf("[custom_planner][makePlanWithOrder] DEBUG: 2 posesOnPathWay_ size(%d)", (int)posesOnPathWay_.size());
} }
} }
return true; return true;
@@ -1130,7 +1101,7 @@ namespace custom_planner
if(posesOnEdge.size()>2){ if(posesOnEdge.size()>2){
for(int i = 0; i<((int)posesOnEdge.size()-1); i++) 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+1].getX(), posesOnEdge[i+1].getY());
posesOnEdge[i].setYaw(theta); posesOnEdge[i].setYaw(theta);
} }
@@ -1140,7 +1111,7 @@ namespace custom_planner
{ {
if(posesOnEdge[0].getX()!=posesOnEdge[1].getX()) 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[1].getX(), posesOnEdge[1].getY());
posesOnEdge[0].setYaw(theta); posesOnEdge[0].setYaw(theta);
posesOnEdge[1].setYaw(theta); posesOnEdge[1].setYaw(theta);
@@ -1154,7 +1125,7 @@ namespace custom_planner
if(posesOnEdge.size()>2){ if(posesOnEdge.size()>2){
for(int i = (int)posesOnEdge.size() -1; i>0; i--) 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-1].getX(), posesOnEdge[i-1].getY());
posesOnEdge[i].setYaw(theta); posesOnEdge[i].setYaw(theta);
} }
@@ -1164,7 +1135,7 @@ namespace custom_planner
{ {
if(posesOnEdge[1].getX()!=posesOnEdge[0].getX()) 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[0].getX(), posesOnEdge[0].getY());
posesOnEdge[1].setYaw(theta); posesOnEdge[1].setYaw(theta);
posesOnEdge[0].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); double yaw_goal = data_convert::getYaw(goal.pose.orientation);
// printf("[custom_planner] DEBUG: angle: %f; yaw_goal: %f",angle, yaw_goal); // 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)); return (delta_angle > (0.5*M_PI));
} }
// Export factory function // 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>(); 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; 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<robot_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> saved_poses; // Lưu các pose đã tính toán
if((int)nurbs_path.size() > 0) if((int)nurbs_path.size() > 0)
{ {
@@ -61,10 +61,10 @@ namespace custom_planner {
for(double t = 0.0; t <= 1.0; t += step) 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)) 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.header.stamp = robot::Time::now();
pose.pose.position = point; pose.pose.position = point;
pose.pose.orientation.x = 0.0; pose.pose.orientation.x = 0.0;
@@ -112,7 +112,7 @@ namespace custom_planner {
double initial_step) double initial_step)
{ {
double length = 0.0; double length = 0.0;
geometry_msgs::Point prev_point, curr_point; robot_geometry_msgs::Point prev_point, curr_point;
std::vector<double> segment_lengths; std::vector<double> segment_lengths;
static double step; static double step;
@@ -137,8 +137,8 @@ namespace custom_planner {
return length; return length;
} }
void MergePathCalc::updatePoseOrientation(std::vector<geometry_msgs::PoseStamped>& saved_poses, void MergePathCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
std::vector<geometry_msgs::PoseStamped>& nurbs_path, std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
bool reverse) bool reverse)
{ {
double yaw; double yaw;
@@ -185,8 +185,10 @@ namespace custom_planner {
if(type == START) 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 // 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; return -1;
} }
@@ -194,7 +196,7 @@ namespace custom_planner {
double min_dist = std::numeric_limits<double>::max(); double min_dist = std::numeric_limits<double>::max();
double distance; 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(), distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(),
posesOnPathWay[i].getY() - pose.getY()); posesOnPathWay[i].getY() - pose.getY());
@@ -221,10 +223,10 @@ namespace custom_planner {
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 === // === 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 // 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]); 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 // Đã đế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 return -1; // Không tìm thấy điểm phù hợp
@@ -312,8 +314,8 @@ namespace custom_planner {
return nearest_idx; return nearest_idx;
} }
bool MergePathCalc::findNURBSControlPoints(vector<geometry_msgs::PoseStamped>& control_points, bool MergePathCalc::findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
const geometry_msgs::PoseStamped& pose, const robot_geometry_msgs::PoseStamped& pose,
std::vector<Pose>& posesOnPathWay, std::vector<Pose>& posesOnPathWay,
point_type type) point_type type)
{ {
@@ -332,15 +334,18 @@ namespace custom_planner {
int idx = findNearestPointOnPath(start_pose, posesOnPathWay, START); 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 // Trường hợp tạo đường thẳng
if(idx == -1) if(idx == -1)
{ {
// FIX: Validate edge_front_end_idx_ trước khi sử dụng // FIX: Validate idx_check 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 false; 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 dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY(); double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();
@@ -349,7 +354,7 @@ namespace custom_planner {
control_points.push_back(pose); control_points.push_back(pose);
// Với bậc 2, thêm 1 điểm trung gian nằm trên đường thẳng // 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.x = start_pose.getX() + dx/2.0;
mid_pose.pose.position.y = start_pose.getY() + dy/2.0; mid_pose.pose.position.y = start_pose.getY() + dy/2.0;
mid_pose.pose.orientation = pose.pose.orientation; mid_pose.pose.orientation = pose.pose.orientation;
@@ -358,7 +363,7 @@ namespace custom_planner {
control_points.push_back(mid_pose); control_points.push_back(mid_pose);
// Thêm điểm cuối // 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.x = posesOnPathWay[start_target_idx_].getX();
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY(); end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw()); 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 // Tạo các control point cho NURBS
control_points.push_back(pose); 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.x = posesOnPathWay[start_target_idx_].getX();
mid_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY(); mid_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
mid_pose.pose.orientation = pose.pose.orientation; mid_pose.pose.orientation = pose.pose.orientation;
@@ -393,7 +398,7 @@ namespace custom_planner {
// end_idx_ = start_target_idx_; // end_idx_ = start_target_idx_;
bool found_suitable_point = false; // FIX: Thêm flag để tránh goto 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 // FIX: Validate bounds trước khi truy cập
if (i >= static_cast<int>(posesOnPathWay.size())) { if (i >= static_cast<int>(posesOnPathWay.size())) {
@@ -409,7 +414,7 @@ namespace custom_planner {
found_suitable_point = true; found_suitable_point = true;
break; 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 // FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
found_suitable_point = false; found_suitable_point = false;
@@ -422,26 +427,28 @@ namespace custom_planner {
// Clear và tạo straight line thay vì goto // Clear và tạo straight line thay vì goto
control_points.clear(); 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_ // 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; 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 dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY(); double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();
control_points.push_back(pose); 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.x = start_pose.getX() + dx/2.0;
mid_pose_straight.pose.position.y = start_pose.getY() + dy/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.pose.orientation = pose.pose.orientation;
mid_pose_straight.header = pose.header; mid_pose_straight.header = pose.header;
control_points.push_back(mid_pose_straight); 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.x = posesOnPathWay[start_target_idx_].getX();
end_pose_straight.pose.position.y = posesOnPathWay[start_target_idx_].getY(); 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.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
@@ -451,7 +458,7 @@ namespace custom_planner {
return true; 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.x = posesOnPathWay[start_target_idx_].getX();
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY(); end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0); 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; goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
// Thêm điểm đầu từ đường đã có // 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.x = posesOnPathWay[goal_target_idx_].getX();
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY(); start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw()); 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(); double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
// Thêm 1 điểm trung gian cho bậc 2 // 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.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.position.y = posesOnPathWay[goal_target_idx_].getY() + dy/2.0;
mid_pose.pose.orientation = start_pose.pose.orientation; mid_pose.pose.orientation = start_pose.pose.orientation;
@@ -553,7 +560,7 @@ namespace custom_planner {
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1; 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.x = posesOnPathWay[goal_target_idx_].getX();
start_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY(); 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.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 dx_straight = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
double dy_straight = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY(); 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.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.position.y = posesOnPathWay[goal_target_idx_].getY() + dy_straight/2.0;
mid_pose_straight.pose.orientation = start_pose_straight.pose.orientation; 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ó // 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.x = posesOnPathWay[goal_target_idx_].getX();
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY(); start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw()); start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
@@ -583,7 +590,7 @@ namespace custom_planner {
start_pose.header = pose.header; start_pose.header = pose.header;
control_points.push_back(start_pose); 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.x = posesOnPathWay[idx].getX();
mid_pose.pose.position.y = posesOnPathWay[idx].getY(); mid_pose.pose.position.y = posesOnPathWay[idx].getY();
mid_pose.pose.orientation = pose.pose.orientation; 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/convert.h>
#include <tf2_ros/buffer.h> #include <tf2_ros/buffer.h>
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
using namespace std; using namespace std;
@@ -41,7 +41,7 @@ bool isThetaValid(double theta)
return result; 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; double delta_angle = 0;
if(isThetaValid(theta)) if(isThetaValid(theta))
@@ -66,7 +66,7 @@ double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose
return delta_angle; 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; double delta_angle = 0;
if(isThetaValid(theta)) 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 // 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<robot_geometry_msgs::PoseStamped> divideSegment(robot_geometry_msgs::PoseStamped& A, robot_geometry_msgs::PoseStamped& B, double d) {
std::vector<geometry_msgs::PoseStamped> Poses; std::vector<robot_geometry_msgs::PoseStamped> Poses;
double xAB = B.pose.position.x - A.pose.position.x; double xAB = B.pose.position.x - A.pose.position.x;
double yAB = B.pose.position.y - A.pose.position.y; double yAB = B.pose.position.y - A.pose.position.y;
double length = sqrt(xAB*xAB + yAB*yAB); 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 // Tính toán tọa độ của các điểm trên đoạn AB
double ratio = d / length; double ratio = d / length;
for (int i = 1; i <= segments; ++i) { 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_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; 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.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 // pose_C: tâm của cung tròn AB
// result_plan: vector chứa plan kết quả // result_plan: vector chứa plan kết quả
bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan, bool makePlanForRetry(std::vector<robot_geometry_msgs::PoseStamped>& current_plan,
int indexOfPoseA, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_B_behind, int indexOfPoseA, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_B_behind,
geometry_msgs::PoseStamped& pose_C, std::vector<geometry_msgs::PoseStamped>& result_plan) robot_geometry_msgs::PoseStamped& pose_C, std::vector<robot_geometry_msgs::PoseStamped>& result_plan)
{ {
bool result = false; bool result = false;
std::vector<geometry_msgs::PoseStamped> PlanRetry_1; std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_1;
std::vector<geometry_msgs::PoseStamped> PlanRetry_2; std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_2;
std::vector<geometry_msgs::PoseStamped> PlanRetry_3; std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_3;
if(current_plan.empty()||current_plan.size()<2) if(current_plan.empty()||current_plan.size()<2)
{ {
@@ -196,7 +196,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
return false; return false;
} }
geometry_msgs::PoseStamped pose_A; robot_geometry_msgs::PoseStamped pose_A;
pose_A = current_plan[indexOfPoseA]; pose_A = current_plan[indexOfPoseA];
// Tính ra PlanRetry_1 điểm retry tại Pose_A // 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), 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_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); planSegment_AB = divideSegment(pose_A, pose_B, 0.1);
PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end()); 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 angle_tmp = angleCA + angleACB*i;
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp); double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
double yP = pose_C.pose.position.y + rCA*sin(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.x = xP;
p.pose.position.y = yP; p.pose.position.y = yP;
p.pose.position.z = 0; 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 angle_tmp = angleCA - angleACB*i;
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp); double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
double yP = pose_C.pose.position.y + rCA*sin(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.x = xP;
p.pose.position.y = yP; p.pose.position.y = yP;
p.pose.position.z = 0; 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 // pose_C: tâm của cung tròn AB
// result_plan: vector chứa plan kết quả // 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, bool makePlanForRetry(robot_geometry_msgs::PoseStamped& pose_A, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_B_behind,
geometry_msgs::PoseStamped& pose_C, std::vector<geometry_msgs::PoseStamped>& result_plan) robot_geometry_msgs::PoseStamped& pose_C, std::vector<robot_geometry_msgs::PoseStamped>& result_plan)
{ {
bool result = false; bool result = false;
std::vector<geometry_msgs::PoseStamped> PlanRetry_1; std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_1;
std::vector<geometry_msgs::PoseStamped> PlanRetry_2; std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_2;
std::vector<geometry_msgs::PoseStamped> PlanRetry_3; 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", 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, 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), 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_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); planSegment_AB = divideSegment(pose_A, pose_B, 0.1);
PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end()); 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 angle_tmp = angleCA + angleACB*i;
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp); double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
double yP = pose_C.pose.position.y + rCA*sin(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.x = xP;
p.pose.position.y = yP; p.pose.position.y = yP;
p.pose.position.z = 0; 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 angle_tmp = angleCA - angleACB*i;
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp); double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
double yP = pose_C.pose.position.y + rCA*sin(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.x = xP;
p.pose.position.y = yP; p.pose.position.y = yP;
p.pose.position.z = 0; 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_B: điểm đích trên cung tròn
// pose_C: tâm của cung tròn AB (kết quả) // 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. // 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), if((computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),